#include <vector>
#include <iostream>

#include <stdlib.h>
#include <time.h>

int MAXSTATES = 5;
int MAXMOVES = 100;
double MPROB=0.5;
int ITERATIONS=100;
const int POP=50;

int grid[40][11];

void fillgrid()
{
  int i,j;
  for(i=0;i<40;i++)
    for(j=0;j<11;j++)
      grid[i][j]=0;

  grid[0][5]=1;
  grid[1][5]=1;
  grid[2][5]=1;
  grid[3][6]=1;
  grid[4][6]=1;
  grid[5][5]=1; 
  grid[6][5]=1;
  grid[7][4]=1;  
  grid[8][4]=1;
  grid[9 ][3]=1;
  grid[10][2]=1;
  grid[11][2]=1;
  grid[12][2]=1;
  grid[13][3]=1; 
  grid[14][4]=1;
  grid[15][4]=1;

  grid[16][4]=1;
  grid[17][5]=1;
  grid[18][6]=1;
  grid[19][6]=1;
  grid[20][7]=1;
  grid[21][7]=1; 
  grid[22][6]=1;
  grid[23][6]=1;

  grid[24][7]=1;
  grid[25][7]=1;
  grid[26][7]=1;
  grid[27][8]=1;
  grid[28][7]=1;
  grid[29][7]=1; 
  grid[30][6]=1;
  grid[31][5]=1;

  grid[32][5]=1;
  grid[33][4]=1;
  grid[34][4]=1;
  grid[35][5]=1;
  grid[36][6]=1;
  grid[37][6]=1; 
  grid[38][7]=1;
  grid[39][6]=1;

}

 


struct trans
{
  int output[3];
  int state[3];
};

class ant
{
public:
  vector<trans> fsm;
  int n;
  int initstate;


  int currentstate;
  int fitness;
  char dir;
  int x,y;
  int moves;

  int com;

  void copy(ant s)
  {
    n=s.n;
    initstate=s.initstate;
    int i,j;
    fsm.erase(fsm.begin(),fsm.end());
    for(i=0;i<n;i++)
      {
	fsm.push_back(s.fsm[i]);
      }


  }



  void printfsm()
  {
    cout << endl << "Printing FSM...";

    cout << endl << "Initstate : " << initstate;

    int i,j;
    for(i=0;i<n;i++)
      {
	cout << endl << "State : " << i << "  ";
	for(j=0;j<3;j++)
	  {
	    cout << " -- " << fsm[i].output[j] << "," << fsm[i].state[j];
	  }
      }

  }


  ant(int size)
  {
    fsm.reserve(MAXSTATES*2);
    n=size;
    fsm.erase(fsm.begin(),fsm.end());
    
    trans t;
    int i,j;
    for(i=0;i<n;i++)
      {
	for(j=0;j<3;j++)
	  {
	    t.output[j]=random()%3;
	    int state=random()%n;
	   
	    t.state[j]=state;
	  }
	fsm.push_back(t);
      }
    initstate=random()%n;
    
    currentstate=initstate;
    fitness=0;
    dir='d';
    x=5;
    y=0 ;
    moves=0;
    com=0;
  }
	
       
  void changeo()
  {
    int q=random()%n;
    int i=random()%3;
    int o=random()%3;
    fsm[q].output[i]=o;
  }

  void changes()
  {
    int q,i,s;
    int j,k;
    int count=0;
    while(1)
      {
	q=random()%n;
	i=random()%3;
	s=random()%n;
	if(fsm[q].state[i]==s && count<n*3)
	  {
	    count++;
	    continue;
	  }
	else
	  {
	    fsm[q].state[i]=s;
	    break;
	  }
      }
    
  }

  void adds()
  {
    trans t;
    n++;
    int j;
    
    for(j=0;j<3;j++)
      {
	t.output[j]=random()%3;
	int state=random()%n;
	t.state[j]=state;
      }
    
    fsm.push_back(t);
    
    //___change i random transitions
    int i=n/2;
    for(int j=0;j<i;j++)
      {
	int s=random()%n;
	int st=random()%3;
	fsm[s].state[st]=n-1;
      }
  }



  void deletes()
  {
    int state=random()%n;
    trans t=fsm[state];
    fsm[state]=fsm[n-1];
    fsm[n-1]=t;

       //    swap(fsm.end(),fsm().begin()+state);

    int i,j;
    for(i=0;i<n-1;i++)
      {
	
	
	for(j=0;j<3;j++)
	  {
	    int s;
	    
	    s=random()%(n-1);
	    
	    if(fsm[i].state[j]==n-1)
	       fsm[i].state[j]=s;
	    
	    
	  }
      }
    fsm.erase(fsm.end());

    n--;
    changeis();

  }
    
    
    

  changeis()
  {
    initstate=random()%n;
  }

  int move()
  {
    int i;
    fillgrid();

    currentstate=initstate;
    fitness=0;
    x=5;
    y=0;
    dir='d';
    moves=0;



    if(com) 
      cout << "\nMoving ant...";

    for(i=0;i<MAXMOVES;i++)
      {
	
	if(grid[y][x]==1)
	  {
	    fitness++;
	    grid[y][x]=0;
	  }
	
	if(com)
	  cout << endl << "x= " << x << "  y=" << y << "  fitness = " << fitness << " dir = " << dir;

	
	//___next transition
	int nx=x,ny=y;
	if(dir=='d')
	  ny++;
	if(dir=='u')
	  ny--;
	if(dir=='l')
	  nx--;
	if(dir=='r')
	  nx++;
	
	int input;

	//________Set proper input
	if(nx<0 || nx>10 || ny<0 || ny>39)
	  input=2;
	else if(grid[ny][nx]==1)
	  input=1;
	else
	  input=0;
	

	int o=fsm[currentstate].output[input];
	currentstate=fsm[currentstate].state[input];
	
	//____Move according to output
	//nx=x;ny=y;
	
	if(o==0) // front
	  {
	    // cout << "\ngoing front" << flush;
	  }

	else if(o==1) // left
	  {
	    //    cout << "\nturning left" << flush;
	    nx=x;ny=y;
	    if(dir=='d') dir='r';
	    else if(dir=='u') dir='l'; 
	    
	    else if(dir=='l') dir='d';
	    else if(dir=='r') dir='u';
	  }
	else if(o==2) // right
	  {
	    //cout << "\nturnung rught" << flush;
	    nx=x;ny=y;
	    if(dir=='d') dir='l';
	    else if(dir=='u') dir='r'; 
	    
	    else if(dir=='l') dir='u';
	    else if(dir=='r') dir='d';
	  }

 
	    
	x=nx;y=ny;
	
	//__see if valid x , y;
	if(x<0 || x >10 || y<0) // die
	  return -1;
	if(y==40)
	  return 1; // __ Completed
	
	moves++;

	if(moves==MAXMOVES) // ___to many moves
	  return 2;
      }


	
  }

  void mutate(ant* array[2][POP],int turn)
  {
    double mp=((double)random())/RAND_MAX;
    int i=random()%POP;
   
	copy(*array[turn][i]);

	if(mp<=MPROB)
	  {
	    //cout << "\nMut" << flush;
	    i=random()%5;
	    
	    if(i==0)
	      //cout << "changeo" << flush
	      changeo();
	    if(i==1)
	      //cout << "changes" << flush,
	      changes();
	    if(i==2 && n<MAXSTATES)
	      //cout << "adds" << flush , 
	      adds();
	    if(i==3 && n>1) //__Must have at least 1 state !
	      //cout << "deletes" << flush ,
	      deletes();
	    if(i==4)
	      //cout << "changeis" << flush,
	      changeis(); 
       
	  }
	    

  }


	
   




};


ant* ants[2][POP];

ant run(int& av)
{
  
  int turn=0;
  int i,j,k;
  
  ant fittest(4);
  
  for(j=0;j<ITERATIONS;j++)
    {
      
      
      //      cout << endl << "Iteration  : " << j+1;
      
      
      
      
      int mf=0;
      av=0;
      //___now move all the ants
      for(i=0;i<POP;i++)
	{
	  //  ants[turn][i]->com=1;
	  int d=ants[turn][i]->move();
	  // if(i==0)
	  //   cout << "\nrvalue : " << d;
	  
	  
	  if(ants[turn][i]->fitness>mf)
	    {
	      mf=ants[turn][i]->fitness;
	      fittest=*ants[turn][i];
	    }
	  av+=ants[turn][i]->fitness;
	  
	  
	}
      av/=POP;

      
      //___Generate next generation
      int turn1=(turn+1)%2;
      
      //      cout << "\nmutating" << flush;
      
      for(i=0;i<POP;i++)
	ants[turn1][i]->mutate(ants,turn);
      //cout << "\nfinished mutating" << flush;
      
      int rv=random()%POP;
      
      ants[turn1][rv]->copy(fittest);
      //cout << "\nassigned fittest" << flush;
      turn=turn1;
           
    }
  

  //  fittest.com=1;
  // fittest.move();
  
  return fittest;
  
}



void main()
{

  srandom(time(NULL));
  ant fittest(4);

  int i,j;
 
  for(int i=0;i<POP;i++)
    {
	ants[0][i]=new ant(3);
	ants[1][i]=new ant(2);
	
    }
  
  int avsum,av;
  /*
  for(int i=0;i<=10;i++)
    {
      ITERATIONS=i*30;
      int fsum=0;
      av=0;avsum=0;
      for(j=0;j<10;j++)
	{
	  fittest=run(av);
	  avsum+=av;
	  fsum+=fittest.fitness;
	}
      cout << endl << ITERATIONS << "\t" << fsum/10 << flush;

    }
  */    

  fittest=run(av);
    //___Got the fittest ant;
  fittest.com=1;
  fittest.move();

  fittest.printfsm();
  cout << "\nFitness : "  << fittest.fitness;
  
  
}