#include <iostream>
#include <fstream>
#include <vector>
#include <ga.h>
#include <cmath>
#include <stdexcept>
#include <algorithm>

# define M_PI 3.141592653589793238462643383279502884
# define ZERO 0.001

using namespace std;

typedef int (*CrossoverMethod)(const GAGenome&, const GAGenome&, GAGenome*, GAGenome*);

namespace {
    vector<float> cables;
    int populationSize = 1000;
    int numberOfGenerations = 1000;
    vector<float> mutation = {0.01, 0.015, 0.02};  //, 0.1, 0.15, 0.2};
    vector<float> crossover = {0.5, 0.6, 0.7};  //0.5, 0.7};
    vector<GABin2DecGenome> bestGenomes;
}

vector<float> readDataFromFile(const char* fileName) {
    ifstream dataFile(fileName);
    float oneLineValue = 0.0;
    vector<float> output;

    while(dataFile >> oneLineValue)
        output.push_back(oneLineValue);
    dataFile.close();

    cout << output.size() << endl;

    return output;
}

void writeSolutionToFile(const char* fileName, GABin2DecGenome bestGenome) {
    ofstream resultsFile, scoresAB;
    vector<float> radius, x, y;
    int numberOfUsedCables = 0;
    float A = 0.0, B = 1.0, phi = 0.0, d;

    scoresAB.open("scores.txt", std::ios_base::app);
    resultsFile.open(fileName);

    resultsFile << cables.size() << endl;

    for (int i = 0; i < cables.size(); i++) {
        if (bestGenome.phenotype(i * 3) != 0) {
            numberOfUsedCables++;
            phi += (M_PI * cables.at(i) * cables.at(i));
        }
    }

    resultsFile << numberOfUsedCables << endl;
    resultsFile << phi << endl;

    for (int i = 0; i < cables.size(); i++) {
        if (bestGenome.phenotype(i * 3) != 0) {
            A++;
            radius.push_back(cables.at(i));
            x.push_back(bestGenome.phenotype((i * 3) + 1));
            y.push_back(bestGenome.phenotype((i * 3) + 2));
            resultsFile << cables.at(i) << " "
                        << bestGenome.phenotype((i * 3) + 1)<< " "
                        << bestGenome.phenotype((i * 3) + 2) << endl;
        }
    }

    // sprawdzenie warunkow zadania
    for (int j = 0; j < radius.size(); j++) {
        for (int i = j + 1; i < radius.size(); i++) {
            d = sqrt(pow((x.at(i) - x.at(j)), 2.0) + pow((y.at(i) - y.at(j)), 2.0));
            if (d < radius.at(i) + radius.at(j)) {
                cout << "Kola zachodza na siebie" << endl;
            }
        }
    }

    for (int i = 0; i < radius.size(); i++) {
        if (x[i] + radius[i] > 1 ||
            x[i] - radius[i] < 0 ||
            y[i] + radius[i] > 1 ||
            y[i] - radius[i] < 0) { cout << "Kolo wychodzi za pole" << endl; }
    }

    if (radius.size() < ceil(0.2 * cables.size())) {
        cout << "za malo uzytych kabli" << endl;
    }

    A /= cables.size();

    scoresAB << A << " " << (B - phi) << endl;

    scoresAB.close();
    resultsFile.close();
}

vector<float> calculateA() {
    vector<float> output;

    for (auto genome : bestGenomes) {
        float A = 0;
        for (int i = 0; i < cables.size(); i++) {
            if (genome.phenotype(i * 3) != 0) {
                A += 1.0;
            }
        }
        output.push_back(A /= cables.size());
    }

    return output;
}

vector<float> calculateB() {
    vector<float> output;

    for (auto genome : bestGenomes) {
        float phi = 0;
        for (int i = 0; i < cables.size(); i++) {
            if (genome.phenotype(i * 3) != 0) {
                phi += (M_PI * cables.at(i) * cables.at(i));
            }
        }
        output.push_back(1.0 - phi);
    }

    return output;
}

int findGenomeFromFirstParetoFrontIndex(vector<float> allA, vector<float> allB) {
    float bestA = -100.0, bestB = -100.0;
    int bestIndex = 0;

    for (int i = 0; i < allB.size(); i++) {//
        if (allB.at(i) > bestB && allA.at(i) >= 0.2) {
            bestB = allB.at(i);
            bestA = allA.at(i);
            bestIndex = i;
        }
        else if (allB.at(i) == bestB && allA.at(i) >= 0.2) {
            if (allA.at(i) > bestA) {
                bestB = allB.at(i);
                bestA = allA.at(i);
                bestIndex = i;
            }
        }
    }

    return bestIndex;
}

void findBestSolution() {
    vector<float> allA = calculateA();
    vector<float> allB = calculateB();

    for (auto a : allA) {
        cout << a << ", ";
    }
    cout << endl << allA.size() << endl;

    for (auto b : allB) {
        cout << b << ", ";
    }
    cout << endl << allB.size() << endl;

    int index = findGenomeFromFirstParetoFrontIndex(allA, allB);

    cout << "BEST: " << allA.at(index) << " " << allB.at(index) << " " << index << endl;
    cout << "MAX B: " << *std::max_element(allB.begin(),allB.end()) << endl;

    writeSolutionToFile("output.txt", bestGenomes.at(index));
}

float objective(GAGenome& c)
{
    vector<float> radius, x, y;
    GABin2DecGenome& genome = (GABin2DecGenome &)c;
    float A = 0.0, B = 1.0, phi = 0.0, output = 0.0, d = 0.0;
    int usedKables = 0;

    for (int i = 0; i < cables.size(); i++) {
        if (genome.phenotype(i * 3) != 0) {
            usedKables++;
            A += 1.0;
            phi += (M_PI * cables.at(i) * cables.at(i));
            radius.push_back(cables.at(i));
            x.push_back(genome.phenotype((i * 3) + 1));
            y.push_back(genome.phenotype((i * 3) + 2));
        }
    }

    A /= static_cast<float>(cables.size());
    B -= phi;
    output += (5 * A) + (10 * B);

    // warunek konieczny zadania
    if (phi >= 1.0)
        return ZERO;

    if (usedKables < ceil(0.2 * cables.size())) {
        output *= (radius.size() / (1.8 * (ceil(0.2 * cables.size()))));
    }

    // czy kable na siebie nachodza
    for (int j = 0; j < radius.size(); j++) {
        for (int i = j + 1; i < radius.size(); i++) {
            d = sqrt(pow((x.at(i) - x.at(j)), 2.0) + pow((y.at(i) - y.at(j)), 2.0));
            if (d < radius.at(i) + radius.at(j))
                output *= (d / (radius.at(i) + radius.at(j)));
        }
    }

    // czy kable mieszcza sie w polu
    for (int i = 0; i < radius.size(); i++) {
        if (x[i] + radius[i] > 1) { output *= abs(x[i] + radius[i] - 1.0);}
        if (x[i] - radius[i] < 0) { output *= abs(x[i] - radius[i]);}
        if (y[i] + radius[i] > 1) { output *= abs(y[i] + radius[i] - 1.0);}
        if (y[i] - radius[i] < 0) { output *= abs(y[i] - radius[i]);}
    }

    return output;
}

GABin2DecGenome run(float mutationProbability, float crossoverProbability, CrossoverMethod crossoverMethod) {
    GABin2DecPhenotype phenotypeMap;

    for (int i = 0; i < cables.size(); i++) {
        phenotypeMap.add(1,  0, 1);
        phenotypeMap.add(10, 0, 1);
        phenotypeMap.add(10, 0, 1);
    }

    GABin2DecGenome genome(phenotypeMap, objective);
    genome.mutator(GA1DBinaryStringGenome::FlipMutator);
    genome.crossover(crossoverMethod);

    GATournamentSelector selector;

    GASimpleGA ga(genome);
    ga.maximize();
    ga.populationSize(populationSize);
    ga.nGenerations(numberOfGenerations);
    ga.pMutation(mutationProbability);
    ga.pCrossover(crossoverProbability);
    ga.selector(selector);

    GASigmaTruncationScaling scaling;
    ga.scaling(scaling);

    ga.initialize();

    while (!ga.done()) { ga.step();}

    GABin2DecGenome best_genome(phenotypeMap, objective);
    best_genome = ga.statistics().bestIndividual();

    return best_genome;
}

int main(int argc, char * argv[]) {
    cables = readDataFromFile("kable.ini");

    vector<CrossoverMethod> crossoverMethods = {&GA1DBinaryStringGenome::OnePointCrossover};

    for (auto crossoverMethod : crossoverMethods) {
        for (float m: mutation) {
            for (float c: crossover) {
                GABin2DecGenome bestGenome = run(m, c, crossoverMethod);
                bestGenomes.push_back(bestGenome);
            }
        }
    }

    findBestSolution();
}