/**
 * DESCRIPTION OF THE FILE
 *
 * @author Michal Kravčenko
 * @date 2.7.18 -
 */

#include "ParticleSwarm.h"
#include "../ErrorFunction/ErrorFunctions.h"

/**
 * TODO
 * domain_bound out_of_range check
 * @param f_dim
 * @param domain_bounds
 * @param F
 */
Particle::Particle(ErrorFunction* ef, double *domain_bounds) {
    //TODO better generating of random numbers
    this->coordinate_dim = ef->get_dimension();

    this->coordinate = new double[this->coordinate_dim];

    this->velocity = new double[this->coordinate_dim];



    for(unsigned int i = 0; i < this->coordinate_dim; ++i){
        this->velocity[i] = (rand() % 100001 - 50000) / (double) 50000;
    }
//    this->r1 = (rand() % 100001) / (double) 100000;
//    this->r2 = (rand() % 100001) / (double) 100000;
    this->r1 = 1.0;
    this->r2 = 1.0;
    this->r3 = 1.0;

    this->optimal_coordinate = new double[this->coordinate_dim];

    this->ef = ef;

    this->domain_bounds = domain_bounds;
    for(unsigned int i = 0; i < this->coordinate_dim; ++i){
        this->coordinate[i] = (rand() % 100001) / (double)100000 + domain_bounds[2 * i] / (domain_bounds[2 * i + 1] - domain_bounds[2 * i]);
        this->optimal_coordinate[i] = this->coordinate[i];
    }

//    printf("coordinate_dim: %d\n", this->coordinate_dim);
    this->optimal_value = this->ef->eval(this->coordinate);

//    this->print_coordinate();
}

Particle::~Particle() {

    if( this->optimal_coordinate ){
        delete [] this->optimal_coordinate;
    }

    if( this->coordinate ){
        delete [] this->coordinate;
    }

    if( this->velocity ){
        delete [] this->velocity;
    }

}

double* Particle::get_coordinate() {
    return this->coordinate;
}

double Particle::get_current_value() {
    return this->current_val;
}

double Particle::get_optimal_value() {
    return this->optimal_value;
}

void Particle::get_optimal_coordinate(std::vector<double> &ref_coordinate) {
    for( unsigned int i = 0; i < this->coordinate_dim; ++i ){
        ref_coordinate[i] = this->optimal_coordinate[i];
    }
}

double Particle::change_coordinate(double w, double c1, double c2, std::vector<double> glob_min_coord, std::vector<std::vector<double>> global_min_vec, double penalty_coef) {

    /**
     * v = w * v + c1r1(p_min_loc - x) + c2r2(p_min_glob - x) + c3r3(random_global_min - x)
     * x = x + v
     */

    double vel_mem;
    double output;
    bool in_domain;
    double compensation_coef = 1;

    /* Choose random global minima */
    std::vector<double> random_global_best(this->coordinate_dim);
    std::random_device rand_dev;
    std::mt19937 engine{rand_dev()};
    std::uniform_int_distribution<int> dist(0, global_min_vec.size() - 1);
    random_global_best = global_min_vec[dist(engine)];

    // TODO use std::sample to choose random vector
    //std::sample(global_min_vec.begin(), global_min_vec.end(), std::back_inserter(random_global_best), 1, std::mt19937{std::random_device{}()});

    for(unsigned int i = 0; i < this->coordinate_dim; ++i){
        vel_mem = w * this->velocity[i]
                + c1 * this->r1 * (this->optimal_coordinate[i] - this->coordinate[i])
                + c2 * this->r2 * (glob_min_coord[i] - this->coordinate[i])
                + (c1+c2)/2 * this->r3 * (random_global_best[i] - this->coordinate[i]);

        do{
            if (this->coordinate[i] + vel_mem > this->domain_bounds[2 * i + 1]) {
                in_domain = false;
                vel_mem = -penalty_coef * compensation_coef * w * vel_mem;
                compensation_coef /= 2;
            } else if (this->coordinate[i] + vel_mem < this->domain_bounds[2 * i]) {
                in_domain = false;
                vel_mem = penalty_coef * compensation_coef * w * vel_mem;
                compensation_coef /= 2;
            } else {
                in_domain = true;
                compensation_coef = 1;
            }
        }while(!in_domain);

        this->velocity[i] = vel_mem;
        this->coordinate[i] += vel_mem;

        output += std::abs(vel_mem);
    }

    vel_mem = this->ef->eval(this->coordinate);
    this->current_val = vel_mem;

    if(vel_mem < this->optimal_value){
        this->optimal_value = vel_mem;
        for(unsigned int i = 0; i < this->coordinate_dim; ++i){
            this->optimal_coordinate[i] = this->coordinate[i];
        }
    }

    return output;
}

void Particle::print_coordinate() {
    for(unsigned int i = 0; i < this->coordinate_dim - 1; ++i){
        printf("%10.8f, ", this->coordinate[i]);
    }
    printf("%10.8f\n", this->coordinate[this->coordinate_dim - 1]);
}

ParticleSwarm::ParticleSwarm(ErrorFunction* ef, double *domain_bounds,
                             double c1, double c2, double w, unsigned int n_particles, unsigned int iter_max) {

    srand(time(NULL));

    //this->func = F;

    this->func_dim = ef->get_dimension();

    this->c1 = c1;

    this->c2 = c2;

    this->c3 = (c1 + c2)/2.0;

    this->w = w;

    this->n_particles = n_particles;

    this->iter_max = iter_max;

    this->particle_swarm = new Particle*[this->n_particles];

    for( unsigned int pi = 0; pi < this->n_particles; ++pi ){
        this->particle_swarm[pi] = new Particle(ef, domain_bounds);
    }

    this->domain_bounds = domain_bounds;


}

ParticleSwarm::~ParticleSwarm() {

    if( this->particle_swarm ){
        for( unsigned int i = 0; i < this->n_particles; ++i ){
            delete this->particle_swarm[i];
        }

        delete [] this->particle_swarm;
    }

}

/**
 *
 * @param gamma
 * @param epsilon
 * @param delta
 */
void ParticleSwarm::optimize( double gamma, double epsilon, double delta) {
    if(epsilon < 0 || gamma < 0 || delta < 0) {
        throw std::invalid_argument("Parameters 'gamma', 'epsilon' and 'delta' must be greater than or equal to zero!");
    }

    unsigned int outer_it = 0;
    Particle *particle;
    std::vector<double> p_min_glob(this->func_dim);
    std::vector<std::vector<double>> global_best_vec;
    double optimal_value = 0.0;

    std::set<Particle*> cluster; //!< Particles in a cluster
    double* coords;
    coords = new double[this->func_dim]; //<! Centroid coordinates

    double tmp_velocity;
    double prev_max_velocity = 0;
    double max_velocity;
    double max_vel_step = 0;
    double prev_max_vel_step;
    double euclidean_dist;

    while( outer_it < this->iter_max ) {
        max_velocity = 0;
        euclidean_dist = 0;

        //////////////////////////////////////////////////
        // Clustering algorithm - termination condition //
        //////////////////////////////////////////////////
        particle = this->determine_optimal_coordinate_and_value(p_min_glob, optimal_value);

        if(std::find(global_best_vec.begin(), global_best_vec.end(), p_min_glob) == global_best_vec.end()) {
            global_best_vec.emplace_back(p_min_glob); // TODO rewrite as std::set
        }

        cluster.insert(particle);

        //for(unsigned int i=0; i < 5; i++) {
            /* Zero AVG coordinates */
            std::fill(coords, coords+this->func_dim, 0);

            /* Looking for a centroid */
            for (auto it : cluster) {

                for (unsigned int di = 0; di < this->func_dim; di++) {
                    coords[di] += it->get_coordinate()[di];
                }
            }

            for(unsigned int di = 0; di < this->func_dim; di++) {
                coords[di] /= cluster.size();
            }

            for(unsigned int pi=0; pi < this->n_particles; pi++) {
                particle = this->particle_swarm[pi];
                tmp_velocity = particle->change_coordinate( this->w, this->c1, this->c2, p_min_glob, global_best_vec);

                if(tmp_velocity > max_velocity) {
                    prev_max_velocity = max_velocity;
                    max_velocity = tmp_velocity;
                }

                /* Looking for nearby particles */
                //printf("iter: %d, pi: %d, euclidean dist: %f\n", outer_it, pi, this->get_euclidean_distance(particle->get_coordinate(), coords, this->func_dim));

                // TODO - only in verbose mode
                // only for info purposes
                euclidean_dist += this->get_euclidean_distance(particle->get_coordinate(), coords, this->func_dim);

                if(this->get_euclidean_distance(particle->get_coordinate(), coords, this->func_dim) < epsilon) {
                    cluster.insert(particle);
                }
            }
        //}

        prev_max_vel_step = max_vel_step;
        max_vel_step = max_velocity - prev_max_velocity;

        //TODO only in verbose mode
        euclidean_dist /= this->n_particles;
        //printf("Iteration %d, avg euclidean distance: %f, cluster percent: %f, f-value: %f\n", outer_it, euclidean_dist,
//               double(cluster.size())/this->n_particles, optimal_value);

//        for(unsigned int i=0; i < this->n_particles; i++) {
//            printf("Particle %d (%f): \n", i, this->particle_swarm[i]->get_current_value());
//            for(unsigned int j=0; j < this->func_dim; j++) {
//                printf("\t%f\n", this->particle_swarm[i]->get_coordinate()[j]);
//            }
//        }

        /* Check if the particles are near to each other AND the maximal velocity is less than 'gamma' */
        if(double(cluster.size())/this->n_particles > delta && std::abs(prev_max_vel_step/max_vel_step) > gamma) {
            break;
        }

        outer_it++;
//        this->w *= 0.99;
    }

    if(outer_it < this->iter_max) {
        /* Convergence reached */

        printf("Found optimum in %d iterations: %10.8f at coordinates: \n", outer_it, optimal_value);
        for (unsigned int i = 0; i <= this->func_dim - 1; ++i) {
            printf("%10.8f \n", p_min_glob[i]);
        }
    } else {
        /* Maximal number of iterations reached */

        printf("Max number of iterations reached (%d)! Found value %10.8f at coordinates: \n", outer_it, optimal_value);
        for (unsigned int i = 0; i <= this->func_dim - 1; ++i) {
            printf("\t%10.8f \n", p_min_glob[i]);
        }
    }

    //delete [] p_min_glob; // TODO
    delete [] coords;
}

Particle* ParticleSwarm::determine_optimal_coordinate_and_value(std::vector<double> &coord, double &val) {

    Particle* p;

    val = this->particle_swarm[0]->get_optimal_value( );
    this->particle_swarm[0]->get_optimal_coordinate(coord);
    p = this->particle_swarm[0];

    for(unsigned int i = 1; i < this->n_particles; ++i){

        double val_m = this->particle_swarm[i]->get_optimal_value( );

        if(val_m < val){
            val = val_m;
            this->particle_swarm[i]->get_optimal_coordinate(coord);
            p = this->particle_swarm[i];
        }
    }

    return p;
}

double* ParticleSwarm::get_centroid_coordinates() {
    double* coords = new double[this->func_dim];
    double* tmp;

    for (unsigned int pi = 0; pi < this->n_particles; pi++) {
        tmp = this->particle_swarm[pi]->get_coordinate();

        for (unsigned int di = 0; di < this->func_dim; di++) {
            coords[di] += tmp[di];
        }
    }

    for(unsigned int di = 0; di < this->func_dim; di++) {
        coords[di] /= this->n_particles;
    }

    return coords;
}

double ParticleSwarm::get_euclidean_distance(double* a, double* b, unsigned int n) {
    double dist = 0;
    for(unsigned int i = 0; i < n; i++) {
        if((a[i]-b[i]) * (a[i]-b[i]) > 1000) {
        }
        dist += ((a[i]-b[i]) * (a[i]-b[i]));
    }
    return std::sqrt(dist);
}