#include "constraintsepsilontakahama2006.h"
#include <algorithm>
#include <vector>
#include <cmath>

ConstraintsEpsilonTakahama2006::ConstraintsEpsilonTakahama2006(int NP, int L, int C,
                                                               const int idxFE, const int idxC)
    : NP(NP), L(L), C(C), idxFE(idxFE), idxC(idxC) {
    this->valueEpsilonConstraint = vEpsilon = 555; //uninitialized!
}

void ConstraintsEpsilonTakahama2006::initEpsilonConstraint(const double *pop) {
        if (C == 0) return;
        //sort by violation
        std::vector<double> epsilons;
        //extract data
        for (int i = 0; i < NP; i++) {
            double ss = 0;
            for (int j = 0; j < C; j++) // sum all powers of all constraints!
                ss += pop[i*L + idxC + j] * pop[i*L + idxC + j];
            ss = sqrt(ss);
            epsilons.push_back(ss);//only the first constraint!
        }

        //sort epsilons, ascending
        sort(epsilons.begin(), epsilons.end());

        //then take the 30%-th
        int epsInd=int(0.7 * NP);
        vEpsilon = epsilons[epsInd];

        this->valueEpsilonConstraint = vEpsilon;
}

void ConstraintsEpsilonTakahama2006::updateEpsilonConstraint(const double *pop, double FEsPercentage) {
    if (C == 0) return;

    const double percFESconstr = 0.2;
    const double Cp = 5;
    const double minEPSavg = 1e-06;
    const double alpha2 = 2;
    const double alpha1 = 0.8;

    double Gperc = float(FEsPercentage);
    double GconstrPerc = Gperc / percFESconstr;

    //only first few percents, the epsilon constraint is relaxed
    if (Gperc < percFESconstr) {
        //calculate vBeta
        //sort by violation
        std::vector<double> epsilons;
        //extract data
        for (int i = 0; i < NP; i++) {
            double ss = 0;
            for (int j = 0; j < C; j++)
                ss += pop[i*L + idxC + j] * pop[i*L + idxC + j];
            ss = sqrt(ss);
            epsilons.push_back(ss);//only the first constraint!
        }

        //sort epsilons, ascending
        sort(epsilons.begin(), epsilons.end());

        //then take the 70%-th
        int epsInd = int(0.3 * NP);//=top 70%-th
        double vBeta = epsilons[epsInd];

        //adjust the vEpsilon coefficient
        if (alpha2 * vBeta < vEpsilon)//if feasibility improved by 2 (=alpha2) for the top 70%-th individual
                vEpsilon = alpha1 * vEpsilon;

        double epsDiminished = vEpsilon * pow(1 - GconstrPerc, Cp);
        if (epsDiminished > minEPSavg)
                this->valueEpsilonConstraint = epsDiminished;
    }
}

bool ConstraintsEpsilonTakahama2006::isBetterFirst(const double *first, const double *second) const {
    const double &f1 = first[idxFE];
    const double &f2 = second[idxFE];
    if (C == 0) {
        //x non-viable, worse!
        if (isnan(f1)) return false;
        //x is viable, always better!
        if (isnan(f2)) return true;

        return f1 < f2; // no constraints
    }

    const double &c1 = first[idxC];
    const double &c2 = second[idxC];
    if (valueEpsilonConstraint > 1e-6
            && c1 > valueEpsilonConstraint
            && c2 > valueEpsilonConstraint)
        return c1 < c2;

    return f1 < f2;
}
