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

#include <cmath>
#include <set>
#include <stdexcept>
#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->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_optimal_value() {
    return this->optimal_value;
}

void Particle::get_optimal_coordinate(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, double *glob_min_coord, double penalty_coef) {

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

    double vel_mem;
    double output;
    bool in_domain;
    double compensation_coef = 1;
    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]);

        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);

    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->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;
    double *p_min_glob = new double[this->func_dim];
    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;

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

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

        for(unsigned int i=0; i < 5; i++) {

            /* 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);

                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));
                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;

        /* Check if the particles are near to each other AND the maximal velocity is less than 'gamma' */
//        printf("cluster size: %ld, n particles: %d, %f\n", cluster.size(), this->n_particles, double(cluster.size())/this->n_particles);
        printf("cluster: %f\n", double(cluster.size())/this->n_particles);
        printf("real gamma: %f\n", std::abs(prev_max_vel_step/max_vel_step));
        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;
    }

    printf("Found optimum in %6d iterations: %10.8f at coordinate: ", outer_it, optimal_value);
    for(unsigned int i = 0; i < this->func_dim - 1; ++i){
        printf("%10.8f, ", p_min_glob[i]);
    }
    printf("%10.8f\n", p_min_glob[this->func_dim - 1]);

    delete [] p_min_glob;
    delete [] coords;
}

Particle* ParticleSwarm::determine_optimal_coordinate_and_value(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);
}