#include <iostream>
#include <math.h>
#include <string>
#include <vector>
#include "MersenneTwister.h"

using namespace std;
double RealPSO(int , int, int );


struct Particle {
   vector<double> pos;
   vector<double> vel;
   vector<double> pBest;
   double pBestValue;
};

struct bParticle {
   vector<int> pos;
   vector<double> vel;
   vector<int> pBest;
   double pBestValue;
};

int decode(int j, int k, vector<int> c){
   int sum, x, n;

   sum = 0;
   n = 1;

   for (x = j; x < k; x++) {
	   if (c[x] == 1) {
		   sum = sum + n;
	   }
	   n = n * 2;
   }
   return(sum);
}


/* 3 variables, 10 bits/variable. */

//i is which individual
//Encoded as 30 bits, 10 bits per variable
double f1(vector<int> curSol){
	register int x;
	double sum;

	double solution = 78.6;
	sum = 0.0;

	for (x = 0; x <= 2; x++) {
		sum = sum + pow(((double)(decode((x * 10), (x * 10) + 10, curSol) - 512) / 100.0), 2.0);
	}

	return(solution - sum);
}


double f1Real(vector<double> sol){

	double sum = 0.0;
	for(unsigned int x=0; x<sol.size(); x++){
		sum += pow(sol[x],2);
	}
	return  sum;
}

double sigMoid(double v){
	return 1/(1+exp(-v));
}

double BinaryPSO(int popSize = 20, int dim = 30, int iterations = 100){

	vector<int> gBest(dim,0);
	double gBestValue = 0.00;//f1Real(gBest);
	double result = 0.0;
	MTRand mt;
	double R1 = mt.randExc();
	double R2 = mt.randExc();
	double C1 = 2;
	double C2 = 2;
	double w = 1;
	double vMax = 1.0;
	double vMin = -1.0;

	srand(time(NULL));

	//Init Pop
	vector<bParticle> swarm;

	for(int x=0; x<popSize; x++){
		bParticle p;
		for(int y=0; y<dim; y++){
			p.pos.push_back(mt.randInt(1));
			p.pBestValue = 0;
		}
		swarm.push_back(p);
	}


	for(int count=0; count < iterations; count++){

		//
		//Evaluate all individuals
		//
		for(unsigned int x=0; x<swarm.size(); x++){


			result = f1(swarm[x].pos);


			//
			//Local Best
			//
			if(result > swarm[x].pBestValue){
				swarm[x].pBest = swarm[x].pos;
				swarm[x].pBestValue = result;
			}
			//
			//Global Best
			//
			if(result > gBestValue){
				gBest = swarm[x].pos;
				gBestValue = result;
			}
		}


		//
		//Update Positions
		//
		R1 = mt.randExc();
		R2 = mt.randExc();
		for(unsigned int x=0; x< swarm.size(); x++){
			for(int y=0; y< dim; y++){
				double newV = (w*swarm[x].vel[y]+ C1*R1*(swarm[x].pBest[y] - swarm[x].pos[y]) + C2*R2*(gBest[y] - swarm[x].pos[y]));
				newV = sigMoid(newV);

				if(newV > vMax){
					newV = vMax;
				}else if(newV < vMin){
					newV = vMax;
				}

				swarm[x].vel[y] = newV;

				if(newV > mt.rand()){
					swarm[x].pos[y] = 1;
				}else{
					swarm[x].pos[y] = 0;
				}
			}
		}

	}

	cout << gBestValue << endl;
	return gBestValue;
}


double RealPSO(int popSize = 20, int dim = 3, int iterations = 100){
	//int popSize = 20, dim = 3, iterations=100;
	vector<double> gBest(dim,0.0);
	double gBestValue = f1Real(gBest);
	double result = 0.0;
	MTRand mt;
	double R1 = mt.randExc();
	double R2 = mt.randExc();
	double C1 = 2;
	double C2 = 2;
	double w = 1;
	double vMax = 6.0;
	double vMin = -6.0;
	double xMax = 5.12, xMin = -5.12;


	srand(time(NULL));

	//Init Pop
	vector<Particle> swarm;

	for(int x=0; x<popSize; x++){
		Particle p;
		for(int y=0; y<dim; y++){
			p.pos.push_back(mt.randInt(1024)/100.0 - 5.12);
			p.pBestValue = 0.0;
		}
		cout << p.pos[0] << " " << p.pos[1] << " " << p.pos[2] << endl;
		swarm.push_back(p);
	}


	for(int count=0; count < iterations; count++){

		//
		//Evaluate all individuals
		//
		for(unsigned int x=0; x<swarm.size(); x++){

			result = f1Real(swarm[x].pos);

			//
			//Local Best
			//
			if(result > swarm[x].pBestValue){
				swarm[x].pBest = swarm[x].pos;
				swarm[x].pBestValue = result;
			}
			//
			//Global Best
			//
			if(result > gBestValue){
				gBest = swarm[x].pos;
				gBestValue = result;
			}
		}


		//
		//Update Positions
		//
		R1 = mt.randExc();
		R2 = mt.randExc();
		for(unsigned int x=0; x< swarm.size(); x++){
			for(int y=0; y< dim; y++){
				double newV = (w*swarm[x].vel[y]+ C1*R1*(swarm[x].pBest[y] - swarm[x].pos[y]) + C2*R2*(gBest[y] - swarm[x].pos[y]));

				if(newV > vMax){
					newV = vMax;
				}else if(newV < vMin){
					newV = vMax;
				}
				swarm[x].vel[y] = newV;
				swarm[x].pos[y] += + swarm[x].vel[y];

				if(swarm[x].pos[y] > xMax){
					swarm[x].pos[y] = xMax;
				}else if(swarm[x].pos[y] < xMin){
					swarm[x].pos[y] = xMin;
				}
			}
		}

	}
	return gBestValue;
}

int main(){

	double r = BinaryPSO(20, 30, 200);
	cout << r << endl;
	return 0;
}