#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "conio.h"
#include <math.h>
#include "UTLab.h"

#define FollowerNumber 3    //Number of followers in the decentralized decision-making problems.
#define PrintNumber   100   //
#define LN 2                // number of variables controled by the leader.
#define FN 2                // number of variables controled by each follower.
#define LM 1                // number of objectives of the leader.
#define FM 1                // number of objectives of each follower.
#define LeaderType 1        // 1=max;-1=min.
#define LeaderGen 500      // maximum generation number of genetic algorithm at the leader's level.
#define PopSize 50          //Population size in genetic algorithm.
#define PrMutation 0.1      //Probability of one chromosome's being selected to mutate.
#define PrCrossover 0.2     //Probability of one chromosome's being selected to crossover.  

static void Simu(unsigned FollowerNo,double Input[9], double Output[2]);

// Define variables and functions related to genetic algorithm procedure of the leader's level. 
double LeaderObj[PopSize+1][LM+1],LeaderChromosome[PopSize+1][LN+1];

static void LeaderInitialization(void);
static int  LeaderConstraintCheck(double LeaderSolution[LN+1]);
static void LeaderEvaluation(unsigned Gen);
static void LeaderSelection(void);
static void LeaderCrossover(void);
static void LeaderMutation(void);
static void LeaderObjective(void);


// Define the routhllete used in selection process of GA procedure.
double Roulette[PopSize+1];
static void RouletteInitialization(void);

static void Simu(unsigned FollowerNo,double Input[3], double Output[2])
{
	unsigned i;

	double xi1,xi2,xi3,xi;
	double z[5001];
	if(FollowerNo==0)
	{
		for(i=1;i<=5000;i++)
		{
			xi=myn(4,1);
			z[i]=sqrt(Input[1]*Input[2]+Input[3]*Input[4]+Input[5]*Input[6]+Input[7]*Input[8]+xi*xi);
		}
		Output[1]=findminn(z,1,5000,500);
	}
	if(FollowerNo==1)
	{
		for(i=1;i<=5000;i++)
		{
			xi1=myn(1,1);
			z[i]=sqrt(Input[1]*Input[1]+Input[3]*Input[3]+2*Input[4]*Input[4]+xi1*xi1);
		}
		Output[1]=findminn(z,1,5000,1000);
	}
	if(FollowerNo==2)
	{
		for(i=1;i<=5000;i++)
		{
			xi2=myn(2,1);
			z[i]=sqrt(Input[1]*Input[1]+Input[3]*Input[3]+2*Input[4]*Input[4]+xi2*xi2);
		}
		Output[1]=findminn(z,1,5000,1000);
	}	
	if(FollowerNo==3)
	{
		for(i=1;i<=5000;i++)
		{
			xi3=myn(3,1);
			z[i]=sqrt(Input[1]*Input[1]+Input[3]*Input[3]+2*Input[4]*Input[4]+xi3*xi3);
		}
		Output[1]=findminn(z,1,5000,1000);
	}
}

static int LeaderConstraintCheck(double LeaderDecision[])
{
	if(0>LeaderDecision[1]||LeaderDecision[1]>10) return 0;
	if(0>LeaderDecision[2]||LeaderDecision[2]>10) return 0;
	if(LeaderDecision[1]+LeaderDecision[2]>10) return 0;
	return 1;
}

static void LeaderInitialization(void)
{
	double LeaderDecision[LN+1];
	unsigned i,j;
	for(i=1; i<=PopSize; i++)
	{
		do
		{
			LeaderDecision[1]=myu(0,10);
			LeaderDecision[2]=myu(0,10);
		}while(LeaderConstraintCheck(LeaderDecision)==0);
		for(j=1;j<=LN;j++) 
			LeaderChromosome[i][j]=LeaderDecision[j];
	}
}

int main()
{
	srand(100);
	//UncertainFunctionAppoximation();
	RouletteInitialization();
	LeaderInitialization();
	LeaderEvaluation(0);
	FILE *fp;
	fp=fopen("RESULT.dat","w");
	unsigned i,j,FollowerNo;
	for(i=1;i<=LeaderGen;i++) 
	{
		LeaderSelection();
		LeaderCrossover();
		LeaderMutation();
		LeaderEvaluation(i);
		if(i%PrintNumber==0)
		{
			printf("\nGeneration NO.%d\n", i);
			fprintf(fp,"\nGeneration NO.%d\n",i);
			printf("Leader's decision x=(");
			fprintf(fp,"Leader's decision x=(");
			double LeaderDecision[LN+1],FollowerDecision[FollowerNumber+1][FN+1];
			for(j=1;j<=LN;j++) 
			{
				LeaderDecision[j]=LeaderChromosome[0][j];
				printf("%f",LeaderChromosome[0][j]);
				if(j<LN) printf(",");
				fprintf(fp,"%f",LeaderChromosome[0][j]);
				if(j<LN) fprintf(fp,",");
			}
			printf("),");
			fprintf(fp,"),");
			printf(" objective F=");
			fprintf(fp," objective F=");
			for(j=1;j<=LM;j++) 
			{
				printf("%f.\n",LeaderObj[0][j]);
				fprintf(fp,"%f.\n",LeaderObj[0][j]);
			}

			FollowerDecision[1][1]=LeaderDecision[2];
			FollowerDecision[1][2]=0;
			FollowerDecision[2][1]=LeaderDecision[2]/2;
			FollowerDecision[2][2]=0;
			FollowerDecision[3][1]=0;
			FollowerDecision[3][2]=LeaderDecision[2]/4;

			double Input[9],Output[2];
			for(j=1;j<=LN;j++) 
			{
				Input[j]=LeaderChromosome[0][j];
			}
			for(FollowerNo=1;FollowerNo<=FollowerNumber;FollowerNo++) 
			{
				for(j=1;j<=FN;j++) 
				{
					Input[LN+j]=FollowerDecision[FollowerNo][j];
				}
				Simu(FollowerNo,Input,Output);
				printf("Follower%d's decision y%d=(",FollowerNo,FollowerNo);
				fprintf(fp,"Follower%d's decision y%d=(",FollowerNo,FollowerNo);
				for(j=1;j<=FN;j++) 
				{
					printf("%f",FollowerDecision[FollowerNo][j]);
					if(j<FN) printf(",");
					fprintf(fp,"%f",FollowerDecision[FollowerNo][j]);
					if(j<FN) fprintf(fp,",");
				}
				printf("),");
				fprintf(fp,"),");
				printf(" objective f%d=",FollowerNo);
				fprintf(fp," objective f%d=",FollowerNo);
				for(j=1;j<=FM;j++) 
				{
					printf("%6.4f.\n",Output[j]);
					fprintf(fp,"%6.4f.\n",Output[j]);
				}
			}

		}
	}
	fclose(fp);
	return 1;
}

//Define the function to calculate a population's objective function value. 
static void LeaderObjective(void)
{
	double LeaderDecision[LN+1],FollowerDecision[FollowerNumber+1][FN+1];
	double Input[MaxDimInput],Output[MaxDimOutput];
	unsigned i,j;
	for(i=1;i<=PopSize;i++) 
	{
		for(j=1;j<=LN;j++) 
		{
			LeaderDecision[j]=LeaderChromosome[i][j];
		}
		FollowerDecision[1][1]=LeaderDecision[2];   FollowerDecision[1][2]=0;
		FollowerDecision[2][1]=LeaderDecision[2]/2; FollowerDecision[2][2]=0;
		FollowerDecision[3][1]=0; FollowerDecision[3][2]=LeaderDecision[2]/4;
		for(j=1;j<=LN;j++) Input[j]=LeaderDecision[j];
		Input[1]=LeaderDecision[1];      Input[2]=LeaderDecision[2];
		Input[3]=FollowerDecision[1][1]; Input[4]=FollowerDecision[1][2];
		Input[5]=FollowerDecision[2][1]; Input[6]=FollowerDecision[2][2];
		Input[7]=FollowerDecision[3][1]; Input[8]=FollowerDecision[3][2];
		Simu(0,Input,Output);
		LeaderObj[i][1]=Output[1];
	}
	for(i=1;i<=PopSize;i++) 
	{
		LeaderObj[i][0]=LeaderObj[i][1];
	}	
}

static void LeaderEvaluation(unsigned Gen)
{
	double temp;
	unsigned i,j,k,label;
	LeaderObjective();
	if(Gen==0)
	{
		for(i=0;i<=LM;i++) LeaderObj[0][i]=LeaderObj[1][i];
		for(i=1;i<=LN;i++) LeaderChromosome[0][i]=LeaderChromosome[1][i];
	}

	for(i=0;i<=PopSize;i++)
	{
		label=0;  
		temp=LeaderObj[i][0];
		for(j=i+1;j<=PopSize;j++)
		{
			if((LeaderType*temp)<(LeaderType*LeaderObj[j][0])) 
			{
				temp=LeaderObj[j][0];
				label=j;
			}
		}
		if(label!=0) 
		{
			for(k=0;k<=LM;k++) 
			{
				temp=LeaderObj[i][k];
				LeaderObj[i][k]=LeaderObj[label][k];
				LeaderObj[label][k]=temp;
			}
			for(j=1;j<=LN;j++) 
			{
				temp=LeaderChromosome[i][j];
				LeaderChromosome[i][j]=LeaderChromosome[label][j];
				LeaderChromosome[label][j]=temp;
			}
		}
	}
}

static void LeaderSelection(void)
{
	double r, temp[PopSize+1][LN+1];
	unsigned i,j,k;
	for(i=1;i<=PopSize;i++) 
	{
		r=myu(0,0.785);
		for(j=0;j<=PopSize; j++) 
		{
			if(r<=Roulette[j]) 
			{
				for(k=1;k<=LN;k++) temp[i][k]=LeaderChromosome[j][k];
				break;
			}
		}
	}
	for(i=1; i<=PopSize; i++)
		for(k=1;k<=LN;k++)
			LeaderChromosome[i][k]=temp[i][k];
}

static void LeaderCrossover(void)
{
	int   i, j, jj, k, pop;
	double r, x[LN+1], y[LN+1];
	pop=PopSize/2;
	for(i=1; i<=pop; i++) 
	{
		if(myu(0,1)>PrCrossover) continue;
		j=1+rand()%PopSize;
		jj=1+rand()%PopSize;
		do
		{
			r=myu(0,1);
			for(k=1;k<=LN;k++) 
			{
				x[k]=r*LeaderChromosome[j][k]+(1-r)*LeaderChromosome[jj][k];
			}
		}while(LeaderConstraintCheck(x)==0);
		do
		{
			r=myu(0,1);
			for(k=1;k<=LN;k++) 
			{
				y[k]=r*LeaderChromosome[jj][k]+(1-r)*LeaderChromosome[j][k];
			}
		}while(LeaderConstraintCheck(y)==0);

		for(k=1;k<=LN;k++) LeaderChromosome[j][k]=x[k];
		for(k=1;k<=LN;k++) LeaderChromosome[jj][k]=y[k];
	}
}

static void LeaderMutation(void)
{
	unsigned i, j, k;
	double x[LN+1],y[LN+1],infty,direction[LN+1];
	double INFTY=10,precision=0.0001;
	for(i=1; i<=PopSize; i++) 
	{
		if(myu(0,1)>PrMutation) continue;
		for(k=1;k<=LN;k++) x[k]=LeaderChromosome[i][k];
		for(k=1; k<=LN; k++)
		{
			if(myu(0,1)<0.5) direction[k]=myu(-1,1);
			else direction[k]=0;
		}
		infty=myu(0,INFTY);
		while(infty>precision) 
		{
			for(j=1;j<=LN;j++) y[j]=x[j]+infty*direction[j];
			if(LeaderConstraintCheck(y)==1) 
			{
				for(k=1;k<=LN;k++) 
					LeaderChromosome[j][k]=y[k];
				break;
			}
			else
			{
				infty=myu(0,infty);
			}
		}
	}
}

static void RouletteInitialization(void)
{
	Roulette[0]=0.05; 
	double temp=0.05;
	for(unsigned i=1;i<=PopSize;i++) 
	{
		temp=temp*0.95; 
		Roulette[i]=Roulette[i-1]+temp;
	}
}