#include "stdio.h"
#include "stdlib.h"
#include "time.h"
#include "math.h"

//rand number defination
#define rdint(i)(rand()%(int)(i))                     //rand integer
#define rdft()(float)((double)rdint(16384)/(16383.0)) //rand floating-point number
#define rnd(a,b)(rdint((int)(b)-(int(a)+1)+(int)(a))  //rand integer between a and b

//macro defination
#define DIMENSION 6
#define POPSIZE 20
#define POP 42

//This program uses cooperative PSO + stochastic PSO to solve the nonlinear equations.
//global vector defination about particle swarm optimization
float W=1.0;
float C1=1.8;
float C2=1.8;
float VMAX=2.0;
float XMIN=-2.0;
float XMAX=2.0;
float P[DIMENSION];  float Q[DIMENSION];
float PBEST;  float QBEST;
int flag=0,flag1=0;  // float out[80];

//define individual structure
struct indi                   
{
	float number[DIMENSION];
	float best[DIMENSION];
	float bestfitness;
	float fitness;
	float speed[DIMENSION];
}individual[POP];  

void initiate(void);
void calculation(int number);
void localbest(int number);
void globalbest(int number);
void generate(int number);

//generate stochastic particle
void generate(int num)
{
   int j;
   for(j=0;j<DIMENSION;j++)
	  individual[num].number[j]=rdft()*(XMAX-XMIN)+XMIN;
   calculation(num);
   individual[num].bestfitness=individual[num].fitness;
   for(j=0;j<DIMENSION;j++)
	  individual[num].best[j]=individual[num].number[j];
}

//initialize
void initiate()
{
	int i,j;
//	srand((time(NULL));

	for(i=0;i<POP;i++)
		for(j=0;j<DIMENSION;j++)
			individual[i].number[j]=rdft()*(XMAX-XMIN)+XMIN; //rand value of individual position
	for(i=0;i<POP;i++)
		for(j=0;j<DIMENSION;j++)
			individual[i].speed[j]=VMAX*rdft();   //rand value of individual speed
	for(i=0;i<POP;i++)
		for(j=0;j<DIMENSION;j++)
			individual[i].best[j]=individual[i].number[j]; //the best individual position
	for(i=0;i<POP;i++)
		calculation(i);
	for(i=0;i<POP;i++)
		individual[i].bestfitness=individual[i].fitness; //bestfitness of individual position
	globalbest(0);
}

void localbest(int number)
{
	int i;
	if(individual[number].bestfitness>individual[number].fitness)
		for(i=0;i<DIMENSION;i++)
			individual[number].best[i]=individual[number].number[i];
		individual[number].bestfitness=individual[number].fitness;
}

//history optimal position modification of swarm
void globalbest(int number)
{
	int i,j;
	float s=0;float s1=0;
	if(number==0)
	{
		s=individual[0].fitness;    s1=individual[17].fitness;
		flag=0;   flag1=0;
		for(i=1;i<POPSIZE;i++)
			if(individual[i].fitness<s)
			{
				s=individual[i].fitness;
				flag=i;
			}
        for(i=0;i<DIMENSION;i++)
            P[i]=individual[flag].number[i];
			PBEST=individual[flag].fitness;

		for(i=17;i<POP;i++)
			if(individual[i].fitness<s1)
			{
				s1=individual[i].fitness;
				flag1=i;
			}
        for(i=0;i<DIMENSION;i++)
			Q[i]=individual[flag1].number[i];
			QBEST=individual[flag1].fitness;
	}
	if(number==1)
	{
		for(i=0;i<POPSIZE;i++)
			if(individual[i].bestfitness<PBEST)
			{
				for(j=0;j<DIMENSION;j++)
				   P[j]=individual[i].best[j];
				PBEST=individual[i].bestfitness;
			}
	}
	if(number==2)
	{
		for(i=17;i<POP;i++)
			if(individual[i].bestfitness<QBEST)
			{
				for(j=0;j<DIMENSION;j++)
					Q[j]=individual[i].best[j];
				QBEST=individual[i].bestfitness;
			}
	}
}

//fitness function (including the nonlinear equations)
void calculation(int num)
{
  float f1,f2,f3,f4,f5,f6;

  f1=fabs(individual[num].number[0]+individual[num].number[1]*individual[num].number[1]
		 *individual[num].number[3]*individual[num].number[5]/4.0+0.75);
  f2=fabs(individual[num].number[1]+0.405*exp(1.0+individual[num].number[0]*individual[num].number[1])
		-1.405);
  f3=fabs(individual[num].number[2]-individual[num].number[1]*individual[num].number[5]/4.0+1.5);

  f4=fabs(individual[num].number[3]-0.605*exp(1-individual[num].number[2]*individual[num].number[2])
		 -0.395);
  f5=fabs(individual[num].number[4]-individual[num].number[1]*individual[num].number[5]/2.0
		 +1.5);
  f6=fabs(individual[num].number[5]-individual[num].number[0]*individual[num].number[4]);

  individual[num].fitness=f1+f2+f3+f4+f5+f6;
}

void main()
{
	int i,j,k,t,m,n,o,q; int flag=-1,fflag;float s,Mu,W1;
	srand(time(NULL));
	clock_t start, finish;
	double duration;          //test time duration of CPSO+SPSO
	int Qr;
	int over=DIMENSION;

	for(q=0;q<50;q++)    //experiments times
	{
		initiate();
		for(k=0;k<DIMENSION;k++)
		{
		   individual[POPSIZE].number[k]=P[k];
		   individual[POPSIZE+1].number[k]=P[k];
		}
		start = clock();
		for(i=0;i<15500;i++)
		{
/*			 Mu=rdft();
			 W1=rdft();
			 W=1.0/(sqrt(2*3.1416)*0.5)*exp(-(W1-Mu)*(W1-Mu)/0.5);   */
			 W=0.9-0.1*i/15499;

			for(t=0;t<3;t++)
			{
			   for(k=0;k<POPSIZE;k++)
			   {
				   for(j=0;j<2;j++)
				   {
					 individual[POPSIZE].number[j]=individual[k].number[j];
					 individual[POPSIZE+1].number[j]=individual[k].best[j];
				   }
				  calculation(POPSIZE);
				  calculation(POPSIZE+1);

				  if(individual[POPSIZE].fitness<individual[POPSIZE+1].fitness)
				  {
					 for(j=0;j<2;j++)
						individual[k].best[j]=individual[POPSIZE].number[j];
					 individual[POPSIZE+1].fitness=individual[POPSIZE].fitness;
				  }
				  if(individual[POPSIZE+1].fitness<PBEST)
				  {
					 for(j=0;j<2;j++)
						P[t]=individual[k].best[t];
					 PBEST=individual[POPSIZE+1].fitness;
				  }
			   }

			   for(k=0;k<POPSIZE;k++)
			   {
				  for(j=0;j<2;j++)
				  {
					 individual[k].speed[j]=W*individual[k].speed[j]+C1*rdft()*(individual[k].best[j]
									-individual[k].number[j])+C2*rdft()*(P[j]-individual[k].number[j]);
					 if(individual[k].speed[j]>VMAX)
						individual[k].speed[j]=VMAX;
						individual[k].number[j]=individual[k].number[j]+individual[k].speed[j];
					 if(individual[k].number[j]<XMIN)
						individual[k].number[j]=XMIN;
					 if(individual[k].number[j]>XMAX)
						individual[k].number[j]=XMAX;
				   }
			   }
			   for(j=0;j<2;j++)
			   {
				  individual[POPSIZE].number[j]=P[j];
				  individual[POPSIZE+1].number[j]=P[j];
			   }
			}

		   W=0.0;
		   for(int p=0;p<10;p++)
		   {
			   Qr=rdint(POPSIZE)+17;
			   for(o=0;o<DIMENSION;o++)
			   {
				  if(individual[Qr].number[o]==Q[o])
				  {over--; continue;}
				  else {over=DIMENSION;break;}
			   }
			   if(over==0) continue;
			   else break;
		   }
		   for(int n=0;n<DIMENSION;n++)
			  individual[Qr].number[n]=P[n];
		   individual[Qr].fitness=PBEST;
		   if(QBEST<PBEST)
		   {
			  for(int m=0;m<DIMENSION;m++)
				P[m]=Q[m];
			  PBEST=QBEST;
		   }
		   if(QBEST>=PBEST)
		   {
			  for(m=0;m<DIMENSION;m++)
				Q[m]=P[m];
			  QBEST=PBEST;
		   }
		   for(k=17;k<POP;k++)
		   {
			  if((individual[k].fitness==individual[k].bestfitness)&&(individual[k].fitness==QBEST))
			  {
				 flag=k;
				 break;
			  }
		   }
		   if(flag!=-1)
			  generate(flag);
		   for(k=17;k<POP;k++)
		   if(k!=flag)
		   {
			  for(j=0;j<DIMENSION;j++)
			  {
				 individual[k].speed[j]=W*individual[k].speed[j]+C1*rdft()*(individual[k].best[j]
									-individual[k].number[j])+C2*rdft()*(P[j]-individual[k].number[j]);
				 if(individual[k].speed[j]>VMAX)
					individual[k].speed[j]=VMAX;
				 individual[k].number[j]=individual[k].number[j]+individual[k].speed[j];
				 if(individual[k].number[j]<XMIN)
					individual[k].number[j]=XMIN;
				 if(individual[k].number[j]>XMAX)
					individual[k].number[j]=XMAX;
			  }
			  calculation(k);
			  localbest(k);
		   }
		   globalbest(2);
		   for(j=0;j<DIMENSION;j++)
		   {
			  individual[POPSIZE].number[j]=Q[j];
			  individual[POPSIZE+1].number[j]=Q[j];
			  P[j]=Q[j];
		   }
		   individual[POPSIZE+1].fitness=QBEST;
		   PBEST=QBEST;
//		   printf("%d,%f\t",i,PBEST);
		   calculation(POPSIZE+1);
		}

		finish = clock();
		duration = (double)(finish-start)/CLOCKS_PER_SEC;

		printf("%d,%f\t",i,PBEST);

		for(j=0;j<DIMENSION;j++)
		  printf("%f\t",individual[POPSIZE+1].number[j]);

		printf( "The time of one experiment is %f seconds\n", duration);
	}

}