int gW = 25;
int gH = 25;
int sW = 300;
int sH = 300;
int cellW, cellH;
int maxC = 55;
int step;
int population=90;
short cgen;

Map m1;
Pop p1;

void setup()
{
  size(sW,sH);
  background(235,242,245);
  cellW = sW/gW;
  cellH = sH/gH;
  step = 0;
  cgen = 0;
  m1 = new Map();
  p1 = new Pop();
  p1.Epoch();
  framerate(10);
}

void loop()
{
  if(step == maxC)
  {
    step = 0;
    p1.Epoch();
    background(235,242,245);
    m1.Reset();
  }
  background(235,242,245);
  m1.drawMap();
  m1.drawStep(p1.gs[p1.best][cgen].instr[step++]);

}

void mousePressed()
{
  step = 0;
  p1.Epoch();
  background(235,242,245);
  m1.Reset();
}

class Pop
{
  Genome gs[][];
  float FitTot;
  float mutRate = .052;
  int best;

  Pop()
  {
    FitTot = 0;
    gs = new Genome[population][2];
    for(int i=0;i<population;i++)
    {
      gs[i][0] = new Genome();
      gs[i][1] = new Genome();
      FitTot += gs[i][0].Process();
    }
  }
  void Epoch()
  {
    int tpop = 0;
    int ma, pa;
    int crossp;
    int tgen = (cgen+1)%2;
    //print("Total Fitness: " + FitTot + "\n");
    //print("------------------------\n");
    while(tpop < population)
    {
      pa = getParents(-1);
      ma = getParents(pa);
      //print("Chose " + pa + " and " + ma + "\n");
      crossp = int(random(maxC));
      for (int i=0; i<crossp;i++)
      {
        gs[tpop][tgen].instr[i] = gs[pa][cgen].instr[i];
        gs[tpop+1][tgen].instr[i] = gs[ma][cgen].instr[i];
      }
      for (int j=crossp; j<maxC;j++)
      {
        gs[tpop][tgen].instr[j] = gs[ma][cgen].instr[j];
        gs[tpop+1][tgen].instr[j] = gs[pa][cgen].instr[j];
      }
      gs[tpop][tgen].Mutate(mutRate);
      gs[tpop++][tgen].Reset();
      gs[tpop][tgen].Mutate(mutRate);
      gs[tpop++][tgen].Reset();
    }
    cgen = short(tgen);
    FitTot = 0;
    float tempfit = 0;
    float bestfit = 0;
    for(int i=0;i<population;i++)
    {
      tempfit = gs[i][cgen].Process();
      if(bestfit < tempfit)
      {
        bestfit = tempfit;
        best = i;
      }
      FitTot += tempfit;
    }

    print("Total Fitness: " + FitTot + "\n");
    print("Best = " + best + "\n");
  }

  int getParents(int p1)
  {
    float lottery;
    float fitsum = 0;
    int cnt=-1;
    if(p1 != -1) lottery = random(0,FitTot-gs[p1][cgen].dFit);
    else lottery = random(0,FitTot);
    while(fitsum < lottery)
    {
      if(++cnt != p1) fitsum += gs[cnt][cgen].dFit;
    }
    return cnt;
  }
}
class Genome
{
  byte[] instr;
  int curx, cury;
  float dFit;

  Genome()
  {
    instr = new byte[maxC];
    for (int y=0; y < maxC; y++)
    {
      instr[y] = (byte)(random(4));
    }
  }
  void Reset()
  {
    curx = 1;
    cury = m1.start;
    dFit = 0;
  }
  void Mutate(float mutRate)
  {
    float t;
    for(int j=0;j<maxC;j++)
    {
      t = random(4);
      if(t/4.0 < mutRate) instr[j] = byte((instr[j] + t) % 4);
    }
  }
  float Process()
  {
    int tx, ty, i;
    curx = 1;
    cury = m1.start;
    for(int j=0;j<maxC;j++)
    {
      i = instr[j];
      tx = curx;
      ty = cury;
      // make into switch statement
      switch(i)
      {
        case 0: curx++; break;   //right
        case 1: curx--; break;   //left
        case 2: cury++; break;   //down
        case 3: cury--; break;   //up
      }
      curx = min(curx,gW-2);
      curx = max(1,curx);
      cury = min(cury,gH-2);
      cury = max(1,cury);
      if (m1.mapar[curx][cury] > 0)
      {
        curx = tx;
        cury = ty;
      }
    }
    dFit = 1 / (1 + dist(curx,cury,gW-2,m1.end));
    //print("Pos = " + curx + "," + cury);
    //print(" Fit = " + dFit + "\n");
    //m1.drawCell(curx,cury);
    return dFit;
  }
}

class Map
{
  int start, end, curx, cury;
  int[][] mapar;
  Map()
  {
    mapar = new int[gW][gH];
    randomize(.05);
    curx = 1;
    cury = start;
  }
  void Reset()
  {
    curx = 1;
    cury = start;
  }
  void drawMap()
  {
    //background(235,242,245);
    stroke(0);
    for (int y=1; y < gH-1; y++)
    {
      for (int x=1; x < gW-1; x++)
      {
        if (mapar[x][y]==0) noFill();
        else if (mapar[x][y]==1) fill(30,115,175);
        else if (mapar[x][y]==2) fill(0,0,0);
        rect(x*cellW,y*cellH,cellW,cellH);
      }
    }
  }
  void drawStep(int i)
  {
    int tx, ty;
    tx = curx;
    ty = cury;
    if(i == 0) curx++;        //right
    else if(i == 1) curx--;   //left
    else if(i == 2) cury++;   //down
    else cury--;                     //up

    if (mapar[curx][cury] > 0)
    {
      curx = tx;
      cury = ty;
    }
    curx = min(curx,gW-2);
    curx = max(1,curx);
    cury = min(cury,gH-2);
    cury = max(1,cury);
    fill(110,110,110);
    rect(curx*cellW,cury*cellH,cellW,cellH);
  }
  void drawCell(int xi, int yi)
  {
    fill(110,110,110);
    rect(xi*cellW,yi*cellH,cellW,cellH);
  }
  void randomize(float density)
  {
    int r1, r2;
    for (int i=0;i<gW*gH*density;i++)
    { r1 = (int)random(gW-2)+1;
      r2 = (int)random(gH-2)+1;
      mapar[r1][r2] = 1;
    }
    start = (int)random(gW-2)+1;
    end = (int)random(gH-2)+1;
    mapar[1][start] = 2;
    mapar[gW-2][end] = 2;
  }
}