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


#define SWARM_SIZE 50

/*How fast to converge [0,1]*/
#define CONVERGENCE .5
#define SWARMTIME 1000
#define DEG 3 /*Actualy degree-1*/
#define POINT_NUM 2
#define SAMPLE_SIZE 50
#define ITERATIONS 10000

/*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;


//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;j++)
	{
	  mpf_set_d(SWARM_POP[i][j],
		    (double) ((double)rand()/ (double)RAND_MAX)); 
	  gmp_printf("%F\n ",SWARM_POP[i][j]);
    
	}
    }
}

//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;
  for(i=1;i<SWARM_SIZE;i++)
    {
      for(j=0;j<POINT_NUM;j++)
	{
	  mpf_set_d(TEMP1,(double)1.0-CONVERGENCE);
	  mpf_mul(TEMP1,SWARM_POP[i][j],TEMP1);
	  mpf_set_d(TEMP2,(double)CONVERGENCE);
	  mpf_mul(TEMP2,TEMP2,SWARM_POP[0][j]);
	  mpf_mul(TEMP2,TEMP2,TEMP1);
	  mpf_set(SWARM_POP[i][j],TEMP2);
	  /*
	  SWARM_POP[i][j]=
	  (CONVERGENCE*SWARM_POP[0][j])+((1.0-CONVERGENCE)*SWARM_POP[i][j]);
	  */
	}
    }
}


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;

  //Initalize everything to zero and precision PREC
  mpf_set_default_prec(PREC);
  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);
  
  
  srand(time(NULL));
  //init population
  evalPointGen();

  /* gmp_printf("\nEVALPOINTGEN\n"); */
/*   for(i=0;i<SWARM_SIZE;i++) */
/*     { */
/*       gmp_printf("\n"); */
/*       for(j=0;j<POINT_NUM;j++) */
/* 	gmp_printf("%F %d,%d,%d\n",SWARM_POP[i][j],j,i,j); */
/*     } */
/*   gmp_printf("\nEND EVALPOINTGEN\n"); */


 //  for(i=0;i<SWARM_SIZE;i++)
//     {
//       cout <<"\n";
//       for(j=0;j<POINT_NUM;j++)
// 	cout<<SWARM_POP[i][j]<<" ";
//     }
  polyGen();
  integrate();
  for(i=0;i<ITERATIONS;i++)
    {
      //polyGen();

      //mpf_set_d(TEMP1,3.1415962);
      //gmp_printf("\n %F",TEMP1);
      mutate();
      //      polyGen();
      //integrate();
      evalPop();
      findFittest();
      //printf("\n");
      mpf_out_str(NULL,10,10,SWARM_FIT[0]);
      printf(":");
      for(j=0;j<POINT_NUM;j++)
	{
	  mpf_out_str(NULL,10,30,SWARM_POP[0][j]);
	  printf(", ");

	  //gmp_printf("%F,",SWARM_POP[0][j]);
	}
      printf("\n");

      //  cout<< SWARM_FIT[0]<<"::";
      //       for(j=0;j<POINT_NUM;j++)
      // 	cout<<SWARM_POP[0][j]<<" ";
      //       cout<<"\n";
      update();
    }
  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;
}