#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <obstack.h>
#include <gmp.h>
#include <signal.h>
//#include <iomanip.h>


#define SWARM_SIZE 50

#define SWARMTIME 1000
#define DEG 5 /*Actualy degree-1*/
#define POINT_NUM 4
#define SAMPLE_SIZE 50
#define ITERATIONS 100000

/*The Precision*/
#define PREC 64



//The co-efficents for the polynomials
mpf_t RAND_COEFS[SAMPLE_SIZE][DEG];
//The evaluation of their integrals
mpf_t RAND_AVES[SAMPLE_SIZE];
//Running fittness of each swarm member
mpf_t SWARM_FIT[SWARM_SIZE];
//Evaluation co-ordinates for each swarm member
mpf_t SWARM_POP[SWARM_SIZE][POINT_NUM];

mpf_t VAL,TEMP1,TEMP2,TEMP3;

mpf_t THE_BEST;

mpf_t BEST_POINT[POINT_NUM];

mpf_t WINDOW_SIZE;


void cnt(int sig)
{
  
  mpf_div_ui(WINDOW_SIZE,WINDOW_SIZE,2);
  printf("\n");
  mpf_out_str(NULL,10,10,WINDOW_SIZE);
  printf("\n");
}


//Get the fittest member of SWARM_POP, swap him to SWARM_POP[0]
void findFittest()
{
  int i,index;
  mpf_set_d(VAL,0.0);

  index=0;
  for(i=1;i<SWARM_SIZE;i++)
    {
      if(mpf_cmp(SWARM_FIT[i],SWARM_FIT[index])<0)
	index=i;
    }


  mpf_swap(SWARM_FIT[index],SWARM_FIT[0]);
  //VAL=SWARM_FIT[index];
  //SWARM_FIT[index]=SWARM_FIT[0];
  //SWARM_FIT[0]=VAL;
  
  for(i=0;i<POINT_NUM;i++)
    {
      mpf_swap(SWARM_POP[index][i],SWARM_POP[0][i]);
      //val=SWARM_POP[index][i];
      //SWARM_POP[index][i]=SWARM_POP[0][i];
      //SWARM_POP[0][i]=VAL;
    }
}


//integrate the randomly generated polynomials from 0 to 1.
//store their value in RAND_AVES
//F(1)-F(0)
//
// ax^2+bx+c
// (a/3) * x^3 +(b/2)x^2 + c*x 
//
void integrate()
{
  int i,j;
  for(i=0;i<SAMPLE_SIZE;i++)
    {
      mpf_set_d(RAND_AVES[i],0.0);
      //RAND_AVES[i]=0.0;
      for(j=0;j<DEG;j++)
	{
	  mpf_set_d(TEMP2,(double)(j+1.0));
	  mpf_div(TEMP1,RAND_COEFS[i][j],TEMP2);
	  mpf_add(RAND_AVES[i],RAND_AVES[i],TEMP1);
	  //RAND_AVES[i]+=(RAND_COEFS[i][j]/((double)j+1.0));
	}
    }
}



//Evaluate the polynomials at the points, average, and penalize them for how far away they are from the actual integral values.
void evalPop()
{
  
 int  i,j,k,l;

  //For each member of the swarm
  for(k=0;k<SWARM_SIZE;k++)
    {
      mpf_set_d(SWARM_FIT[k],0.0);//SWARM_FIT[k]=0.0;
      for(i=0;i<SAMPLE_SIZE;i++)
	{
	  mpf_set_d(VAL,0.0);//VAL=0;
	  for(j=0;j<DEG;j++)
	    {
	      for(l=0;l<POINT_NUM;l++)
		{
		  mpf_pow_ui(TEMP1,SWARM_POP[k][l],(long)j);
		  mpf_mul(TEMP1, TEMP1,RAND_COEFS[i][j]);
		  mpf_add(VAL,VAL,TEMP1);
		 //VAL+= RAND_COEFS[i][j]*mpf_pow_ui(SWARM_POP[k][l],j);
		
		}
	    }
	  mpf_set_d(TEMP1,(double)POINT_NUM);
	  mpf_div(TEMP1,VAL,TEMP1);
	  mpf_sub(TEMP1,TEMP1,RAND_AVES[i]);
	  //VAL=(VAL/(double)POINT_NUM)-RAND_AVES[i];
	  mpf_abs(TEMP1,TEMP1);
	  mpf_add(SWARM_FIT[k],SWARM_FIT[k],TEMP1);
	}
    }
} 

void polyGen()
{
  int i,j;
  for(i=0;i<SAMPLE_SIZE;i++)
    {
      for(j=0;j<DEG;j++)
	mpf_set_d(RAND_COEFS[i][j], (((double)rand())/ ((double)RAND_MAX)*2.0) -1.0);
    }
}
void evalPointGen()
{
  int i,j;
  for(i=0;i<SWARM_SIZE;i++)
    {
      for(j=0;j<(POINT_NUM/2);j++)
	{
	  //mpf_set_d(TEMP1,(double)rand());
	  //mpf_set_d(TEMP2,(double)RAND_MAX);
	  //mpf_div(SWARM_POP[i][j],TEMP1,TEMP2);  
	  mpf_set_d(SWARM_POP[i][j],(double) ((double)rand()/ (double)RAND_MAX)); 
	}
    }
}

//Evaluate each member of the population to see how close they were to the
//actual answers


//Swarm points towards the fittest member
void update()
{
  int i,j,k;
  //mpf_t BEST_POINT[POINT_NUM]
  for(i=0;i<POINT_NUM;i++)
    mpf_set(BEST_POINT[i],SWARM_POP[0][i]);

  evalPointGen();
  mpf_set_d(TEMP2,0.5);
  for(i=0;i<SWARM_SIZE;i++)
    {
      for(j=0;j<(POINT_NUM/2);j++)
	{
	  mpf_sub(TEMP1, SWARM_POP[i][j],TEMP2);
	  mpf_mul(TEMP1,TEMP1,WINDOW_SIZE);
	  mpf_add(SWARM_POP[i][j],BEST_POINT[j],TEMP1);
	  if(mpf_sgn(SWARM_POP[i][j])<0)
	    mpf_set_d(SWARM_POP[i][j],0.0);
	  //SWARM_POP[i][j]=BEST_POINT[j]+((SWARM_POP[i][j]-0.5)*WINDOW_SIZE);
	}
      for(j=0;j<(POINT_NUM/2);j++)
	mpf_ui_sub(SWARM_POP[i][j+(POINT_NUM/2)],1,SWARM_POP[i][j]);
      
      
      for(i=0;i<POINT_NUM;i++)
	mpf_set(SWARM_POP[0][i],BEST_POINT[i]);
    }
}

  void evalSingle(int k)
    {
      int i,j,l;
  mpf_set_d(SWARM_FIT[k],0.0);//SWARM_FIT[k]=0.0;
  for(i=0;i<SAMPLE_SIZE;i++)
    {
      mpf_set_d(VAL,0.0);//VAL=0;
      for(j=0;j<DEG;j++)
	{
	  for(l=0;l<POINT_NUM;l++)
	    {
	      mpf_pow_ui(TEMP1,SWARM_POP[k][l],(long)j);
	      mpf_mul(TEMP1, TEMP1,RAND_COEFS[i][j]);
	      mpf_add(VAL,VAL,TEMP1);
	      //VAL+= RAND_COEFS[i][j]*mpf_pow_ui(SWARM_POP[k][l],j);
	      
	    }
	}
      mpf_set_d(TEMP1,(double)POINT_NUM);
      mpf_div(TEMP1,VAL,TEMP1);
      mpf_sub(TEMP1,TEMP1,RAND_AVES[i]);
      //VAL=(VAL/(double)POINT_NUM)-RAND_AVES[i];
      mpf_abs(TEMP1,TEMP1);
      mpf_add(SWARM_FIT[k],SWARM_FIT[k],TEMP1);
    }
}





//play with the vectors....
//add/sub WINDOW_SIZE*.01 to each
void twiddle()
{
  int i,j,k;
  mpf_set_d(TEMP1,.01);
  mpf_mul(TEMP1,WINDOW_SIZE,TEMP1);
  mpf_set_d(TEMP2,-.01);
  mpf_mul(TEMP2,WINDOW_SIZE,TEMP2);
  
  for(i=0;i<DEG;i++)
    mpf_set(SWARM_POP[1][i],SWARM_POP[0][i]);      
  for(i=0;i<DEG;i++)
    {
      mpf_add(SWARM_POP[1][i],SWARM_POP[1][i],TEMP1);
      evalSingle(1);
      if(mpf_cmp(SWARM_FIT[1],SWARM_FIT[0])<0)
	{
	  mpf_set(SWARM_POP[0][i],SWARM_POP[1][i]);
	  mpf_set(SWARM_FIT[0],SWARM_FIT[1]);
	}
      mpf_add(SWARM_POP[1][i],SWARM_POP[1][i],TEMP2);
      evalSingle(1);
      if(mpf_cmp(SWARM_FIT[1],SWARM_FIT[0])<0)
	{
	  mpf_set(SWARM_POP[0][i],SWARM_POP[1][i]);
	  mpf_set(SWARM_FIT[0],SWARM_FIT[1]);
	}
      mpf_set(SWARM_POP[1][i],SWARM_POP[0][i]);
    }
}



void mutate()
{
  int i,j;
  double pMutate=.01;
  
  for(i=1;i<SWARM_SIZE;i++)
    {
      if(((double)rand()/(double)RAND_MAX)< pMutate)
	{
	  for(j=0;j<POINT_NUM;j++)
	   {
	     mpf_set_d(SWARM_POP[i][j],((((double)rand())/ (double)RAND_MAX)));
	     //SWARM_POP[i][j]= ((((double)rand())/ (double)RAND_MAX)); 
	   }
	}
    }
}



int main()
{
  int i,j,k;
  int bestCount;


  signal(SIGINT,cnt);

  //Initalize everything to zero and precision PREC
  mpf_set_default_prec(PREC);
  mpf_init(WINDOW_SIZE);

  for(i=0;i<POINT_NUM;i++)
    mpf_init(BEST_POINT[i]);

  for(i=0;i<SAMPLE_SIZE;i++)
    {
      mpf_init(RAND_AVES[i]);
      for(j=0;j<DEG;j++)
	mpf_init(RAND_COEFS[i][j]);
    }
  for(i=0;i<SWARM_SIZE;i++)
    {
      mpf_init(SWARM_FIT[i]);
      for(j=0;j<POINT_NUM;j++)
	mpf_init(SWARM_POP[i][j]);
    }
  mpf_init(VAL);mpf_init(TEMP1);mpf_init(TEMP2);mpf_init(TEMP3);
  
  mpf_init(THE_BEST);

  mpf_set_d(WINDOW_SIZE,0.5);


  srand(time(NULL));
  //init population
  evalPointGen();
  polyGen();
  integrate();
  bestCount=0;
  mpf_set_d(THE_BEST,999999999);
  for(i=0;i<ITERATIONS;i++)
    {

      for(j=0;j<10;j++)
	{
	  evalPop();
	  findFittest();
	  update();
	}
      //twiddle();
      evalPop();
      findFittest();
      mpf_out_str(NULL,10,10,SWARM_FIT[0]);
      printf("::");
      for(j=0;j<POINT_NUM;j++)
	{
	  mpf_out_str(NULL,10,50,SWARM_POP[0][j]);
	  printf("\t");
	}
      printf("\n");
      update();
      
      bestCount++;
      if(mpf_cmp(THE_BEST,SWARM_FIT[0])>0)
	{
	  mpf_set(THE_BEST,SWARM_FIT[0]);
	  bestCount=0;
	}
      if(bestCount>20)
	{cnt(1); bestCount=0;}

    }
  printf("Best Points:\n");

  for(i=0;i<POINT_NUM;i++)
    {
      //gmp_printf("%.*F\n\n ",PREC,SWARM_POP[0][i]
      mpf_out_str(NULL,10,30,SWARM_POP[0][i]);
      printf("\n\n");
    }

  // for(i=0;i<POINT_NUM;i++)
//     cout<<SWARM_POP[0][i]<<" ";
//   cout<<"\nSample Polynomial x^0 x^1 x^2...::";
//   for(i=0;i<DEG;i++)
//     cout<< RAND_COEFS[0][i]<<" ";
//   cout<<"\nItegral[0,1]:"<<RAND_AVES[0];

  //loop:

  // init polynomials, evaluate members, swarm, mutate


  //return the best population member found
  return 0;
}