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

#include "ParticleSwarm.h"

Particle::Particle(unsigned int f_dim, double *domain_bounds, double (*F)(double*)) {
    this->coordinate_dim = f_dim;

    this->coordinate = new double[f_dim];

    this->velocity = new double[f_dim];



    for(unsigned int i = 0; i < f_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[f_dim];

    this->f = F;

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

    this->optimal_value = this->f(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_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) {

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

    double vel_mem, output = 0.0;
    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]);

        if( this->coordinate[i] + vel_mem > this->domain_bounds[2 * i + 1] ){
//            vel_mem = vel_mem * (-1.0) * (vel_mem - this->domain_bounds[2 * i + 1]) / (this->domain_bounds[2 * i + 1] - this->velocity[i]);
            vel_mem = -0.25 * w * vel_mem;
        }
        else if( this->coordinate[i] + vel_mem < this->domain_bounds[2 * i] ){
//            vel_mem = vel_mem * (-1.0) * (this->domain_bounds[2 * i] - vel_mem) / (this->velocity[i] - this->domain_bounds[2 * i]);
            vel_mem = -0.25 * w * vel_mem;
        }

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

        output += vel_mem * vel_mem;
    }

    vel_mem = this->f(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];
        }
    }

//    this->print_coordinate();

    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(double (*F)(double*), unsigned int f_dim, 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 = f_dim;

    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( f_dim, domain_bounds, F);
    }

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

}

void ParticleSwarm::optimize( double epsilon ) {

    epsilon *= epsilon;

    unsigned int outer_it = 0;
    double optimum_step = epsilon * 10;
    Particle *particle;
    double *p_min_glob = new double[this->func_dim];
    double optimal_value;

    this->determine_optimal_coordinate_and_value(p_min_glob, optimal_value);
    double optimum_prev;

    double max_velocity, mem;
    printf("%10d: max. velocity^2: %10.8f, optimum: %10.5f\n", outer_it, max_velocity, optimal_value);
    while( outer_it < this->iter_max && optimum_step > epsilon ){

        optimum_prev = optimal_value;


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

            if(mem > max_velocity){
                max_velocity = mem;
            }
        }

        this->determine_optimal_coordinate_and_value(p_min_glob, optimal_value);

//        optimum_step = std::abs( optimal_value - optimum_prev );
        optimum_step = max_velocity;

        printf("%10d: max. velocity^2: %10.8f, optimum: %10.5f\n", outer_it, max_velocity, optimal_value);

        outer_it++;
//        this->w *= 0.999;
    }
//    printf("\n");

    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;

}

void ParticleSwarm::determine_optimal_coordinate_and_value(double *coord, double &val) {

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

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