float mx=0;
float my=0;
float px;
float py;

int roadWidth = 15;
int roadOffSX = 42;
int roadOffWY = 42;
int roadOffNX = roadOffSX+roadWidth;
int roadOffEY = roadOffWY+roadWidth;

int roadleftX = (int)(roadOffSX-(roadWidth/2.0));
int roadcenterX = (int)((roadOffSX+roadOffNX)/2.0);
int roadrightX = (int)(roadOffNX+(roadWidth/2.0));

int roadtopY = (int)(roadOffWY-(roadWidth/2.0));
int roadcenterY = (int)((roadOffWY+roadOffEY)/2.0);
int roadbottomY = (int)(roadOffEY+(roadWidth/2.0));

int vehRow  = 5;

int numX = 6;
int numY = 9;

int fWidth = 2200;
int fHeight = 1000;

int gridX = (fWidth-(roadcenterX *2))/(numX-1);
int gridY = (fHeight-(roadcenterY *2))/(numY-1);

int numVeh=(((numX*vehRow)*2)+((numY*vehRow)*2));
int vehCount =0;

vehicle test[] = new vehicle[numVeh];
int targetCount = 0;
float targets[][] = new float[10000][3];

boolean threeD = true;
int numBBlock = 10;
float buildingSizeX =  ((gridX-(roadWidth*2))/(numBBlock-1));
float buildingSizeY =  ((gridY-(roadWidth*2))/(numBBlock-1));

Font metaBold;

void setup() {

  size(1200,600);

  metaBold = loadFont("Meta-Bold.vlw.gz");
  textFont(metaBold, 22);

  // setup target buildings
  for (int j=0;j<numY-1;j++) {
    for (int i=0;i<numX-1;i++) {
      for(int g=0;g<numBBlock-1;g++) {
        targets[targetCount][0] = roadrightX+(i*gridX);
        targets[targetCount][1] = roadbottomY+(j*gridY)+(((gridY-(roadWidth*2))/(numBBlock-1))*g);
        targetCount++;
        targets[targetCount][0] = roadrightX+(i*gridX)+(((gridX-(roadWidth*2))/(numBBlock-1))*g);
        targets[targetCount][1] = roadbottomY+(j*gridY);
        targetCount++;
        targets[targetCount][0] = roadleftX+((i+1)*gridX);
        targets[targetCount][1] = roadbottomY+(j*gridY)+(((gridY-(roadWidth*2))/(numBBlock-1))*g);
        targetCount++;
        targets[targetCount][0] = roadrightX+((i)*gridX)+(((gridX-(roadWidth*2))/(numBBlock-1))*g);
        targets[targetCount][1] = roadtopY+((j+1)*gridY);
        targetCount++;
      }
    }
  }

  for (int j=0;j<numX;j++) {
    for (int i=0;i<vehRow;i++) {
      test[vehCount] = new vehicle(roadOffSX+(j*gridX),15+((fHeight*0.87/vehRow)*i),PI/2.0,random(3)+1,8,4,(int)random(targetCount),vehCount);
      vehCount++;
    }

    for (int i=0;i<vehRow;i++) {
      test[vehCount] = new vehicle(roadOffNX+(j*gridX),15+((fHeight*0.87/vehRow)*i),PI*1.5,random(3)+1,8,4,(int)random(targetCount),vehCount);
      vehCount++;
    }
  }

  for (int j=0;j<numY;j++) {

    for (int i=0;i<vehRow;i++) {
      test[vehCount] = new vehicle(15+((fWidth*0.87/vehRow)*i),roadOffWY+(j*gridY),PI,random(5)+1,8,4,(int)random(targetCount),vehCount);
      vehCount++;
    }

    for (int i=0;i<vehRow;i++) {
      test[vehCount] = new vehicle(15+((fWidth*0.87/vehRow)*i),roadOffEY+(j*gridY),0,random(5)+1,8,4,(int)random(targetCount),vehCount);
      vehCount++;
    }
  }

}

void loop() {
  background(0);
  stroke(60);
  for (int i=0;i<numVeh;i++) {
    line(test[i].pos[0],test[i].pos[1],test[i].target[0],test[i].target[1]);
  }
  stroke(140);

  for (int j=0;j<numX;j++) {
    line(roadleftX+(j*gridX),0,roadleftX+(j*gridX),fHeight);
    line(roadcenterX+(j*gridX),0,roadcenterX+(j*gridX),fHeight);
    line(roadrightX+(j*gridX),0,roadrightX+(j*gridX),fHeight);
  }
  for (int j=0;j<numY;j++) {
    line(0,roadtopY+(j*gridY),fWidth,roadtopY+(j*gridY));
    line(0,roadcenterY+(j*gridY),fWidth,roadcenterY+(j*gridY));
    line(0,roadbottomY+(j*gridY),fWidth,roadbottomY+(j*gridY));
  }

  for (int j=0;j<numX;j++) {
    for (int i=0;i<numY;i++) {
      rectMode(CORNER);
      fill(0,144,0);
      stroke(0,144,0);
      rect(roadleftX+(j*gridX),roadcenterY+(i*gridY),roadWidth,roadWidth);
      fill(0,145,0);
      stroke(0,145,0);
      rect(roadcenterX+(j*gridX),roadcenterY+(i*gridY),roadWidth,roadWidth);
      fill(0,146,0);
      stroke(0,146,0);
      rect(roadcenterX+(j*gridX),roadtopY+(i*gridY),roadWidth,roadWidth);
      fill(0,147,0);
      stroke(0,147,0);
      rect(roadleftX+(j*gridX),roadtopY+(i*gridY),roadWidth,roadWidth);
    }
  }

  for (int i=0;i<numVeh;i++) {
    //test[i].setDir(0.05-random(0.1));
    test[i].checkTurn();
    test[i].update(i);

    if (!threeD) test[i].draw();
  }

  if (threeD) {
    background(0);
    push();
    
    translate(width*0.5,height*0.5,-800);
    //if(mousePressed) rotateX(mouseY/200.0);
    // if(mousePressed) rotateZ(mouseX/200.0);
    // translate(-fWidth*0.5,-fHeight*0.5,-0);

    if (mousePressed == true) {
      rotateX(((py-mouseY+my)*PI/250.0));
      rotateZ((px-mouseX+mx)*PI/250.0);
    } else {
      rotateX((my)*PI/250.0);
      rotateZ((mx)*PI/250.0);
    }

    translate(-fWidth*0.5,-fHeight*0.5,-0);
    //rotateY(mouseY/100.0);

    //buildings
    push();
    translate(fWidth/2.0,fHeight/2.0,-16);
    scale(fWidth,fHeight,30);
    fill(90);
    stroke(140);
    //box(1);
    pop();
    targetCount =0;

    float offsetX = buildingSizeX/2.0;
    float offsetY = buildingSizeY/2.0;

    for (int j=0;j<numY-1;j++) {
      for (int i=0;i<numX-1;i++) {
        for(int g=0;g<numBBlock-1;g++) {

          push();
          translate(targets[targetCount][0]+offsetX,targets[targetCount][1]+offsetY,targets[targetCount][2]*7);
          scale(buildingSizeX,buildingSizeY,targets[targetCount][2]*14);
          fill(100+targets[targetCount][2]*20);
          box(1);
          pop();
          targetCount++;
          push();
          translate(targets[targetCount][0]+offsetX,targets[targetCount][1]+offsetY,targets[targetCount][2]*7);
          scale(buildingSizeX,buildingSizeY,targets[targetCount][2]*14);
          fill(100+targets[targetCount][2]*20);
          box(1);
          pop();
          targetCount++;
          push();
          translate(targets[targetCount][0]-offsetX,targets[targetCount][1]+offsetY,targets[targetCount][2]*7);
          scale(buildingSizeX,buildingSizeY,targets[targetCount][2]*14);
          fill(100+targets[targetCount][2]*20);
          box(1);
          pop();
          targetCount++;
          push();
          translate(targets[targetCount][0]+offsetX,targets[targetCount][1]-offsetY,targets[targetCount][2]*7);
          scale(buildingSizeX,buildingSizeY,targets[targetCount][2]*14);
          fill(100+targets[targetCount][2]*20);
          box(1);
          pop();
          targetCount++;
        }
      }
    }

    for (int j=0;j<numX;j++) {
      line(roadleftX+(j*gridX),0,roadleftX+(j*gridX),fHeight);
      line(roadcenterX+(j*gridX),0,roadcenterX+(j*gridX),fHeight);
      line(roadrightX+(j*gridX),0,roadrightX+(j*gridX),fHeight);
    }
    for (int j=0;j<numY;j++) {
      line(0,roadtopY+(j*gridY),fWidth,roadtopY+(j*gridY));
      line(0,roadcenterY+(j*gridY),fWidth,roadcenterY+(j*gridY));
      line(0,roadbottomY+(j*gridY),fWidth,roadbottomY+(j*gridY));
    }
    for (int i=0;i<numVeh;i++) {
      test[i].draw3d();
    }
    pop();
  }
}

class vehicle{
  float pos[] = new float[2];
  float speed = 0;
  float rota = 0;
  float targetsp;
  float vLength;
  float vWidth;
  boolean turning;
  float target[] = new float[2];
  color br =  color(0,0,0);
  float a;
  int vNum;
  color r = color(144,0,0);
  color g1 = color(0,144,0);
  color g2 = color(0,145,0);
  color g3 = color(0,146,0);
  color g4 = color(0,147,0);

  vehicle (float x, float y, float rot, float sp,float vL,float vW, int tCount,int tnum) {
    pos[0] = x;
    pos[1] = y;
    //dir[0] = dx;
    //dir[1] = dy;
    rota = rot;
    speed = sp;
    targetsp = sp;
    vLength = (int)(vL/2);
    vWidth =(int)(vW/2);
    target[0] = targets[tCount][0];
    target[1] = targets[tCount][1];
    targets[tCount][2]++;
    turning = false;
    vNum = tnum;

  }

  void update(int num)
  {
    if (targetsp>speed) speed = speed *1.1;
    else speed = targetsp;
    if (targetsp>4) targetsp = 4;
    float tempx = pos[0];
    float tempy = pos[1];
    boolean pass = false;
    int failed=0;
  if((get((int)pos[0],(int)pos[1])==g1)||(get((int)pos[0],(int)pos[1])==g2)||(get((int)pos[0],(int)pos[1])==g3)||(get((int)pos[0],(int)pos[1])==g4)) {speed =1;}

    while (pass!=true) {
      boolean testthem = true;
      pos[0]= pos[0]+(cos(rota) * speed*3);
      pos[1]= pos[1]+(sin(rota) * speed*3);
      if (pos[0]>fWidth ) pos[0] = 0;
      if (pos[1]>fHeight) pos[1] = 0;
      if (pos[0]<0) pos[0] = fWidth ;
      if (pos[1]<0) pos[1] = fHeight;
    if((get((int)pos[0],(int)pos[1])==g1)||(get((int)pos[0],(int)pos[1])==g2)||(get((int)pos[0],(int)pos[1])==g3)||(get((int)pos[0],(int)pos[1])==g4)) {speed =1;}

      //float spacex = abs(pos[0]-tempx)/2.0;
      //float spacey = abs(pos[1]-tempy)/2.0;
      //if (spacex>vLength*6) spacex=vLength*6;
      //if (spacey>vLength*6) spacey=vLength*6;

      float xl = test[num].pos[0]-vLength;
      float yt = test[num].pos[1]-vLength;
      float xr = test[num].pos[0]+vLength;
      float yb = test[num].pos[1]+vLength;

      rectMode(CORNER);
      noFill();
      stroke(100);
      //rect(xl,yt,vLength*2,vLength*2);
      rect(xl,yt,(vLength)*2,(vLength)*2);
      fill(255);
      for (int i = 0;i<numVeh;i++) {
        if (i!=num){
          float xl2 = test[i].pos[0]-vLength;
          float yt2 = test[i].pos[1]-vLength;
          float xr2 = test[i].pos[0]+vLength;
          float yb2 = test[i].pos[1]+vLength;
          if (((xl>=xl2)&&(xl<=xr2)&&(yt>=yt2)&&(yt<=yb2))||
          ((xl>=xl2)&&(xl<=xr2)&&(yb>=yt2)&&(yb<=yb2))||
          ((xr>=xl2)&&(xr<=xr2)&&(yt>=yt2)&&(yt<=yb2))||
          ((xr>=xl2)&&(xr<=xr2)&&(yb>=yt2)&&(yb<=yb2)))
          {
            //print ("crossed");
            noFill();
            stroke(255,0,0);
            br = color(255,0,0);
            rect(xl,yt,vLength*2,vLength*2);
            fill(255);
            stroke(100);
            testthem = false;
            break;
          }
          //else br=color(0,0,0);
        }
      }
      if (testthem == false) {
        pos[0] = tempx;
        pos[1] = tempy;
        speed = speed*0.9;
        if (speed<0.1) speed = 1;
        //setDir(0.1);
        failed++;
        targetsp *=0.9999;
        br = color(255,0,0);
        if (targetsp<4) targetsp=3;
        if (failed>20) pass=true;
        //println("break");
      }
      else {
        pos[0]= tempx+(cos(rota) * speed);
        pos[1]= tempy+(sin(rota) * speed);
        if (pos[0]>fWidth ) pos[0] = 0;
        if (pos[1]>fHeight) pos[1] = 0;
        if (pos[0]<0) pos[0] = fWidth ;
        if (pos[1]<0) pos[1] = fHeight;
        targetsp *=1.0005;
        if (failed>=1) br = color(255,0,0);
        else br=color(0,0,0);
        pass = true;

      }
    }
  }

  void draw() {
    push();
    translate(pos[0],pos[1]);
    rotate(rota);
    rectMode(CENTER_DIAMETER);
    if (vNum==0) fill(20,200,0);
    else fill(240);
    rect(0,0,vLength*2,vWidth*2);
    pop();
    //line(pos[0],pos[1],target[0],target[1]);
  }

  void draw3d() {
    push();
    translate(pos[0],pos[1],2);
    rotate(rota);
    //rectMode(CENTER_DIAMETER);
    //rect(0,0,vLength*2,vWidth*2);
    float ratio = vLength/vWidth;
    scale(ratio,1,0.5);
    box(vWidth);
    push();
    translate(-vWidth/2.0,-vWidth/2.0,0);
    stroke(br);
    scale(1.0/ratio,1,2);
    box(1);
    pop();
    push();
    translate(-vWidth/2.0,vWidth/2.0,0);
    stroke(br);
    scale(1.0/ratio,1,2);
    box(1);
    pop();
    stroke(100);
    pop();
    //line(pos[0],pos[1],target[0],target[1]);
  }

  void setPos(float x, float y) {
    pos[0] = x;
    pos[1] = y;
  }
  void setDir(float rot)
  {
    rota+=rot;
    if (rota<0) rota = TWO_PI+rota;
    rota = rota%TWO_PI;
    if ((rota>(PI-0.1))&&(rota<(PI+0.1))) rota =PI;
    if ((rota>((0.5*PI)-0.1))&&(rota<((0.5*PI)+0.1))) rota =(0.5*PI);
    if ((rota>((1.5*PI)-0.1))&&(rota<((1.5*PI)+0.1))) rota =(1.5*PI);
    if ((rota>(TWO_PI-0.1))&&(rota<(TWO_PI+0.1))) rota =TWO_PI;

  }

  float testDir(float rot)
  {

    a = atan2(target[1]-pos[1], target[0]-pos[0]);
    float tmpRota = rota;
    tmpRota += rot;
    //if (tmpRota<0) tmpRota = TWO_PI+tmpRota;
    //tmpRota = tmpRota%TWO_PI;
    return abs(tmpRota-a);
  }

  void checkTurn()
  {
    float comp[] = new float[3];
    comp[0] = testDir(-0.5*PI);
    comp[1] = testDir(0);
    comp[2] = testDir(0.5*PI);
    float smallest =20;
    int pointer =0;
    for (int i=0;i<3;i++) {
      if (smallest>comp[i]) {
        smallest = comp[i];
        pointer = i;
      }
    }

    if (pointer==0) line(pos[0]+(cos(rota-(0.5*PI))*4),pos[1]+(sin(rota-(0.5*PI))*10),pos[0] +(cos(rota-(0.5*PI))*10) , pos[1]+(sin(rota-(0.5*PI))*10));
    if (pointer==2) line(pos[0]+(cos(rota+(0.5*PI))*4),pos[1]+(sin(rota+(0.5*PI))*10),pos[0] +(cos(rota+(0.5*PI))*10),   pos[1]+(sin(rota+(0.5*PI))*10));

    //text(pointer, pos[0]+40, pos[1]+40);
    //println(pointer);
    if (vNum ==0)fill(get((int)pos[0],(int)pos[1]));;
    if (vNum ==0) rect(700,100,50,50);
    fill(140);
    //if ((vNum==0)&&(turning)) println("true");

    if(((get((int)pos[0],(int)pos[1]) == g1)&&(rota==0))&&(!turning)&&(pointer==2))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numX;j++) {
        if ((int)pos[0]>=((roadOffSX-2)+(j*gridX))&&((int)pos[0]<=((roadOffSX+2)+(j*gridX)))){
          setDir(PI*0.5);
          setPos(roadOffSX+(j*gridX),pos[1]);
          turning = true;
          br = color(255,0,0);
          //println("case1");
          break;
        }
      }
    }

    else if(((get((int)pos[0],(int)pos[1]) == g2)&&(rota==0))&&(!turning)&&(pointer==0))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numX;j++) {
        if ((int)pos[0]>=((roadOffNX-2)+(j*gridX))&&((int)pos[0]<=((roadOffNX+2)+(j*gridX)))){
          setDir(-PI*0.5);
          setPos(roadOffNX+(j*gridX),pos[1]);
          turning = true;
          br = color(255,0,0);
          //println("case3");
          break;
        }
      }
    }

    else if(((get((int)pos[0],(int)pos[1]) == g4)&&(rota==PI))&&(!turning)&&(pointer==0))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numX;j++) {
        if ((int)pos[0]>=((roadOffSX-2)+(j*gridX))&&((int)pos[0]<=((roadOffSX+2)+(j*gridX)))){
          setDir(-PI*0.5);
          setPos(roadOffSX+(j*gridX),pos[1]);
          turning = true;
          br = color(255,0,0);
          //println("case2");
          break;
        }
      }
    }

    //else if((get((int)pos[0],(int)pos[1]) == g4)&&(vNum==0)) println("why not "+pointer+" "+frame);

    else if(((get((int)pos[0],(int)pos[1]) == g3)&&(rota==PI))&&(!turning)&&(pointer==2))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numX;j++) {
        if ((int)pos[0]>=((roadOffNX-2)+(j*gridX))&&((int)pos[0]<=((roadOffNX+2)+(j*gridX)))){
          setDir(PI*0.5);
          setPos(roadOffNX+(j*gridX),pos[1]);
          turning = true;
          br = color(255,0,0);
          //println("case4");
          break;
        }
      }
    }

    else if(((get((int)pos[0],(int)pos[1]) == g1)&&(rota==0.5*PI))&&(!turning)&&(pointer==0))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numY;j++) {
        if ((int)pos[1]>=((roadOffEY-2)+(j*gridY))&&((int)pos[1]<=((roadOffEY+2)+(j*gridY)))) {
          setDir(-PI*0.5);
          setPos(pos[0],roadOffEY+(j*gridY));
          turning = true;
          br = color(255,0,0);
          //println("case8");
          break;
        }
      }
    }

    else if(((get((int)pos[0],(int)pos[1]) == g4)&&(rota==0.5*PI))&&(!turning)&&(pointer==2))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numY;j++) {
        if ((int)pos[1]>=((roadOffWY-2)+(j*gridY))&&((int)pos[1]<=((roadOffWY+2)+(j*gridY)))) {
          setDir(PI*0.5);
          setPos(pos[0],roadOffWY+(j*gridY));
          turning = true;
          br = color(255,0,0);
          //println("case5");
          break;
        }
      }
    }

    else  if(((get((int)pos[0],(int)pos[1]) == g3)&&(rota==1.5*PI))&&(!turning)&&(pointer==0))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numY;j++) {
        if ((int)pos[1]>=((roadOffWY-2)+(j*gridY))&&((int)pos[1]<=((roadOffWY+2)+(j*gridY)))) {
          setDir(-PI*0.5);
          setPos(pos[0],(roadOffWY+(j*gridY)));
          turning = true;
          br = color(255,0,0);
          ///println("case7");
          break;
        }
      }
    }

    else  if(((get((int)pos[0],(int)pos[1]) == g2)&&(rota==1.5*PI))&&(!turning)&&(pointer==2))
    {
      speed = 1;
      if (speed<0.1) speed = 1;
      for(int j=0;j<numY;j++) {
        if ((int)pos[1]>=((roadOffEY-2)+(j*gridY))&&((int)pos[1]<=((roadOffEY+2)+(j*gridY)))) {
          setDir(PI*0.5);
          setPos(pos[0],(roadOffEY+(j*gridY)));
          turning = true;
          br = color(255,0,0);
          //println("case6");
          break;
        }
      }
    }

    else if ((get((int)pos[0],(int)pos[1]) != g1) &&(get((int)pos[0],(int)pos[1]) != g2)&&(get((int)pos[0],(int)pos[1]) != g3)&&(get((int)pos[0],(int)pos[1]) != g4))
    {
      //if (vNum==0) println("reset");
      turning=false;
      br = color(0,0,0);
    }
  }

}
void mouseReleased()
{
  if ((pmouseX>70) && (pmouseY<500)) {
    mx = px-pmouseX+mx;
    my = py-pmouseY+my;
  }
}