#pragma once

// #define _USE_MATH_DEFINES
#include <algorithm>
#include <cmath>
// #include <execution>
#include <numbers>
#include <random>

/*

a, sigma

*/

// #define POP_SIZE 30
// #define NUM_IT 25
// #define COL_SIZE 16

// using Column = double;

static std::random_device random_device;
static std::mt19937 gen(random_device());
static std::uniform_real_distribution<> dis01(0., 1.);
static std::uniform_real_distribution<> disDelta2(-2., 2.);

template<typename T, size_t COL_SIZE = 16>
struct Item {
    double a, sigma; // E, sigma = sqrt(D)
    T* column;

    double W;

    Item() {}

    Item(double a_, double s, T * column_) : a(a_), sigma(s), column(column_) {
        update();
    }

    double gauss(double t) {
        return std::exp(std::pow((t - a) / sigma,
                                 2) /
                        2) /
               std::sqrt(2 * std::numbers::pi) /
               sigma;
    }

    double F() { // objective function
        double s = 0;
        int x0 = std::floor(.5 + a - 3 * sigma);
        double x1 = std::round(a + 3 * sigma);

        for (int j = x0; j <= x1; j++)
            s += std::pow(gauss(j) - column[j], 2);

        return s;
    }

    void update() {
        W = F();
    }

    // action

    Item move() {
        double a1 = a + disDelta2(gen);
        double sigma1 = sigma * (1 + disDelta2(gen)); // a: ~  +- 2 pixel

        return Item(a1, sigma1, column);
    }

    // a = q * a1 + (1 - q) * a2,   sigma = q * s1 + (1 - q) * s2
    Item crossover(const Item& other) {
        double q = dis01(gen);
        double a_ = q * a + (1 - q) * other.a;
        double sigma_ = q * sigma + (1 - q) * other.sigma;

        return Item(a_, sigma_, column);
    }
};

template <typename T = uint16_t,
         size_t POP_SIZE = 30,
         size_t COL_SIZE = 16,
         size_t NUM_IT = 25,
         double maxW = .01>
struct Algo {
    T * column;
    using I = Item<T, COL_SIZE>;
    std::vector<I> population;
    std::uniform_real_distribution<double> disA { 0., double(1.) };

    double targetW;

    Algo(T * column_): column(column_) {
        init();
    }

    I getNewItem() {
        double a = rand() % (COL_SIZE * 1000) / 1000.;
        double sigma = rand() % (COL_SIZE * 100) / 1000.;

        return I(a, sigma, column);
    }

    void init() {
        for (size_t i = 0; i < POP_SIZE; i++) {
            population.push_back(getNewItem());
        }
    }

    bool stopCondition() {
        // return population[0].W <= targetW;
        return population[0].W <= maxW;
    }

    I run() {
        for (int it = 0; it < NUM_IT; it++) {
            work();

            // if (stopCondition())
                // break;
        }

        return population[0];
    }

    void work() {
        move();
        crossover();
        addNew();

        sort();

        remove();
    }

    void sort() {
        // sort vector ASC
        std::sort(population.begin(), population.end(),
                  [](const auto & a, const auto & b)
                  {
                      return a.W < b.W;
                  });
    }

    void remove() {
        population.erase(population.begin() + POP_SIZE, population.end());
    }

    void move() {
        for (I& item : population) {
            population.push_back(item.move());
        }
    }

    void addNew() {
        for (int i = 0; i < 5; i++) {
            population.push_back(getNewItem());
        }
    }

    void crossover() {
        for (int i1 = 0; i1 < 4; i1++) {

            for (int i2 = i1 + 1; i2 < 4; i2++) {
                I& x1 = population[i1];
                I& x2 = population[i2];
                 // a = q * a1 + (1 - q) * a2,   sigma = q * s1 + (1 - q) * s2
                population.push_back(x1.crossover(x2));
            }
        }
    }
};