vehicle test[] = new vehicle[200];
int numVeh=200;
int numX = 6;
int numY = 6;
int vehNum = 0;
int gridX = 100;
int gridY = 100;
int targetCount = 0;
float targets[][] = new float[1000][3];

void setup() {
  size(600,600);
  
  for (int j=0;j<numX-1;j++) {
    for (int i=0;i<numY-1;i++) {
      for(int g=0;g<10;g++) {
         targets[targetCount][0] = 65+(i*gridX);
         targets[targetCount][1] = 65+(j*gridY)+((70/9)*g);
         targetCount++;
         targets[targetCount][0] = 65+(i*gridX)+((70/9)*g);
         targets[targetCount][1] = 65+(j*gridY);
         targetCount++;
         targets[targetCount][0] = 35+((i+1)*gridX);
         targets[targetCount][1] = 65+(j*gridY)+((70/9)*g);
         targetCount++;
         targets[targetCount][0] = 65+((i)*gridX)+((70/9)*g);
         targets[targetCount][1] = 35+((j+1)*gridY);
         targetCount++;
       }  
    }
  }
    
  
  for (int j=0;j<5;j++) {
    for (int i=0;i<10;i++) {
      test[vehNum] = new vehicle(42+(j*100),15+(50*i),PI/2.0,random(5)+1,(int)random(targetCount),vehNum);
    vehNum++;
    }
    for (int i=0;i<10;i++) {
      test[vehNum] = new vehicle(15+(50*i),42+(j*100),PI,random(5)+1,(int)random(targetCount),vehNum);
    vehNum++;
    }
    for (int i=0;i<10;i++) {
      test[vehNum] = new vehicle(58+(j*100),15+(50*i),PI*1.5,random(5)+1,(int)random(targetCount),vehNum);
    vehNum++;
    }
    for (int i=0;i<10;i++) {
      test[vehNum] = new vehicle(15+(50*i),58+(j*100),0,random(5)+1,(int)random(targetCount),vehNum);
    vehNum++;
    }
  }
}

void loop() {
  background(0);
  for (int j=0;j<6;j++) {
    for (int i=0;i<6;i++) {
      fill(0,147,0);
      rectMode(CORNER);
      rect(35+(j*100),35+(i*100),15,15);
      fill(0,145,0);
      rect(50+(j*100),50+(i*100),15,15);
      fill(0,146,0);
      rect(50+(j*100),35+(i*100),15,15);
      fill(0,144,0);
      rect(35+(j*100),50+(i*100),15,15);
    }
  }

  for (int j=0;j<6;j++) {
    line(35+(j*100),0,35+(j*100),600);
    line(50+(j*100),0,50+(j*100),600);
    line(65+(j*100),0,65+(j*100),600);
  }
  for (int j=0;j<6;j++) {
    line(0,35+(j*100),600,35+(j*100));
    line(0,50+(j*100),600,50+(j*100));
    line(0,65+(j*100),600,65+(j*100));
  }
  for (int i=0;i<numVeh;i++) {
    //test[i].setDir(0.05-random(0.1));
    test[i].update(i);
    test[i].checkTurn();
    test[i].draw();
    
  }
  background(0);
  push();
  translate(0,100,0);
  rotateX(1.0);
  //rotateY(mouseY/100.0);
  for (int j=0;j<6;j++) {
    line(35+(j*100),0,35+(j*100),600);
    line(50+(j*100),0,50+(j*100),600);
    line(65+(j*100),0,65+(j*100),600);
  }
  for (int j=0;j<6;j++) {
    line(0,35+(j*100),600,35+(j*100));
    line(0,50+(j*100),600,50+(j*100));
    line(0,65+(j*100),600,65+(j*100));
  }
  push();
  translate(300,300,-3);
  scale(1,1,0.01);
  
  box(600);
  pop();
  targetCount =0;
  for (int j=0;j<numX-1;j++) {
    for (int i=0;i<numY-1;i++) {
      for(int g=0;g<10;g++) {
      push();
      translate(targets[targetCount][0]+3,targets[targetCount][1]+3);
      scale(1,1,targets[targetCount][2]*2);
      box(7);
      pop();
      targetCount++;
         push();
      translate(targets[targetCount][0]+3,targets[targetCount][1]+3);
      scale(1,1,targets[targetCount][2]*2);
      box(7);
      pop();
      targetCount++;
        push();
      translate(targets[targetCount][0]-3,targets[targetCount][1]+3);
      scale(1,1,targets[targetCount][2]*2);
      box(7);
      pop();
      targetCount++;
        push();
      translate(targets[targetCount][0]+3,targets[targetCount][1]-3);
      scale(1,1,targets[targetCount][2]*2);
      box(7);
      pop();
      targetCount++;
       }  
    }
  }
  
  
  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);

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

  void update(int num)
  {
    if (targetsp>speed) speed = speed *1.1;
    else speed = targetsp;
    if (targetsp>3) targetsp = 3;
    float tempx = pos[0];
    float tempy = pos[1];
    boolean pass = false;
    int failed=0;

    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]>width ) pos[0] = 0;
      if (pos[1]>height) pos[1] = 0;
      if (pos[0]<0) pos[0] = width ;
      if (pos[1]<0) pos[1] = height;
      
      //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;
        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]>width ) pos[0] = 0;
        if (pos[1]>height) pos[1] = 0;
        if (pos[0]<0) pos[0] = width ;
        if (pos[1]<0) pos[1] = height;
        targetsp *=1.0005;
        pass = true;

      }
    }
  }

  void draw() {
    push();
    translate(pos[0],pos[1]);
    rotate(rota);
    rectMode(CENTER_DIAMETER);
    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);
    scale(2,1,0.5);
    box(vLength);
    translate(-vLength/2.0,0,0);
    stroke(br);
    scale(0.5,1,2);
    box(1);
    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;
    
  }

  void checkTurn()
  {
    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);

    if(((get((int)pos[0],(int)pos[1]) == g1)&&(rota==0))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[0]>=(40+(j*100))&&((int)pos[0]<=(44+(j*100)))){
          setDir(PI*0.5);
          setPos(42+(j*100),pos[1]);
          turning = true;
          break;
          }
      }
    }
    else if(((get((int)pos[0],(int)pos[1]) == g4)&&(rota==PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[0]>=(40+(j*100))&&((int)pos[0]<=(44+(j*100)))) {
          setDir(-PI*0.5);
          setPos(42+(j*100),pos[1]);
          turning = true;
          break;
          }
      }
    }

    else if(((get((int)pos[0],(int)pos[1]) == g2)&&(rota==0))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[0]>=(56+(j*100))&&((int)pos[0]<=(60+(j*100)))) {
          setDir(-PI*0.5);
          setPos(58+(j*100),pos[1]);
          turning = true;
          break;
          }
      }
    }
    
    else if(((get((int)pos[0],(int)pos[1]) == g3)&&(rota==PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[0]>=(56+(j*100))&&((int)pos[0]<=(60+(j*100)))) {
          setDir(PI*0.5);
          setPos(58+(j*100),pos[1]);
          turning = true;
          break;
          }
      }
    }
    
    else if(((get((int)pos[0],(int)pos[1]) == g4)&&(rota==0.5*PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[1]>=(40+(j*100))&&((int)pos[1]<=(44+(j*100)))) {
          setDir(PI*0.5);
          setPos(pos[0],42+(j*100));
          turning = true;
          break;
          }
      }
    }
    
    else  if(((get((int)pos[0],(int)pos[1]) == g2)&&(rota==1.5*PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[1]>=(56+(j*100))&&((int)pos[1]<=(60+(j*100)))) {
          setDir(PI*0.5);
          setPos(pos[0],(58+(j*100)));
          turning = true;
          //println("happening");
          break;
          
          }
      }
    }
    
    else  if(((get((int)pos[0],(int)pos[1]) == g3)&&(rota==1.5*PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[1]>=(40+(j*100))&&((int)pos[1]<=(44+(j*100)))) {
          setDir(-PI*0.5);
          setPos(pos[0],(42+(j*100)));
          turning = true;
          //println("happening");
          break;
          
          }
      }
    }
    
    else if(((get((int)pos[0],(int)pos[1]) == g4)&&(rota==0.5*PI))&&(!turning))
    {
      speed = speed*0.6;
      if (speed<0.1) speed = 1;
      for(int j=0;j<6;j++) {
        if ((int)pos[1]>=(56+(j*100))&&((int)pos[1]<=(60+(j*100)))) {
          setDir(-PI*0.5);
          setPos(pos[0],58+(j*100));
          turning = true;
          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))
    {
    turning=false;
    
   } 
  }

}