/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Flower Constancy algortihm created by Pawel Kopciewicz, February 2018   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% An example usage of Flower Constancy algorithm for given function model %
% fitting (data from distorted Gauss distribution with C, mi, sigma).                                                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <iostream>
#include <chrono>
#include <random>
#include <cmath>
using namespace std;

double FCA(int points, double *x, double *y){

int N=250;      //Number of algorithm iterations
int n=20;       //Number of flowers

int P=points;       //Number of points we fit a model to

double pop[50][4]; double pop2[50][4];        //Arrays for flower positions
double cost[n]; double cost2;                 //Vectors for position evaluations

//Random initial solution:
for(int i=0; i<n; i++){           
	pop[i][0]=5+5*((double)rand()/RAND_MAX-0.5);    //Amplitude
	pop[i][1]=5+5*((double)rand()/RAND_MAX-0.5);    //Mean
	pop[i][2]=5+5*((double)rand()/RAND_MAX-0.5);    //Standard Deviation
}

//Cost evaluation:
for(int j=0; j<n; j++){                
	cost[j]=0;
	for(int k=0; k<P; k++){            
		double I=pop[j][0]*exp(-(x[k]-pop[j][1])*(x[k]-pop[j][1])/(2*pop[j][2]*pop[j][2]));
		cost[j]=cost[j]+(I-y[k])*(I-y[k]);
    }
}

//Flower Constancy Algorithm
for(int i=0; i<N; i++){
	for(int j=0; j<n; j++){
	    
	    //Pollination:
        double e=2*(double)rand()/RAND_MAX-1;
        int z1=round((n-1)*(double)rand()/RAND_MAX); int z2=round((n-1)*(double)rand()/RAND_MAX);
                
		for(int p=0; p<3; p++)
            pop2[j][p]=pop[j][p]+e*(pop[z1][p]-pop[z2][p]);   
        
            //New solution cost evaluation:
            //(Add constraint-handling code here if needed)
            
        cost2=0;
        for(int k=0; k<P; k++){            
		    double I=pop2[j][0]*exp(-(x[k]-pop2[j][1])*(x[k]-pop2[j][1])/(2*pop2[j][2]*pop2[j][2]));
		    cost2=cost2+(I-y[k])*(I-y[k]);
        }
        
        //If new solution is better, update it
        if(cost2<cost[j]){
            for(int it=0; it<3; it++)    
                pop[j][it]=pop2[j][it];                
		    cost[j]=cost2;	
        }
	} 
} 
//End solutions cost evaluation
double value=cost[0];   //One of initial position costs to comparison
int no;
for(int j=0; j<n; j++){                
	cost[j]=0;
	for(int k=0; k<P; k++){            
		double I=pop[j][0]*exp(-(x[k]-pop[j][1])*(x[k]-pop[j][1])/(2*pop[j][2]*pop[j][2]));
		cost[j]=cost[j]+(I-y[k])*(I-y[k]);
    }
    if(cost[j]<value){
        value=cost[j];
        no=j;
    }
}

double C=pop[no][0];
double mi=pop[no][1];
double sigma=pop[no][2];


cout<<"Variance: "<<value/P<<endl;
cout<<"Gauss amplitude: "<<C<<endl;
cout<<"Gauss mean(shift): "<<mi<<endl;
cout<<"Gauss std: "<<sigma<<endl;

}

//////////////wywolanie funkcji///////////////////////
int main(){

//Generating data from Gauss:
int points=30;     //Number of points
double x[points]; double y[points];
double A=4; double mi=6; double sg=1;  //Gauss parameters
for(int k=0; k<points; k++){            
		x[k]=-2+(double)k*0.5;
		y[k]=A*exp(-(x[k]-mi)*(x[k]-mi)/(2*sg*sg));
    }

FCA(30,x,y); 

return 0;
}
