// car accidents
// Written June 2006 by Jarek Rossignac

color red=color(256,100,100), green=color(100,256,100), blue=color(100,100,256), yellow=color(256,230,100), grey=color(100,100,100), lightGrey=color(60,60,60);
int nr=2; 
car C [] = new car [nr];
int ns=4; 
street S [] = new street [ns];
vec I=new vec (1,0);

boolean displayPath = true;   // shows whether control points for car paths are displayed
boolean animating = false;   // set to true while the scenario is animating

float t = 0.0;                // track time

class street {
  float w=80;                                      // street width
  pt B=new pt(700,100), F=new pt(600,700);         // front and back end-points
  
  boolean pickedB=false, pickedF=false, picked=false;
  
  street (float bx, float by, float fx, float fy, float pw) {
    B.x=bx;
    B.y=by;
    F.x=fx;
    F.y=fy;
    w=pw;
  };
  
  void paint() {
    fill(200,200,100); 
    noStroke(); 
    vec N=B.vecTo(F).left(); 
    N.unit(); 
    N.mul(w);
    
    beginShape(); 
    B.movedBy(N).vert();  
    F.movedBy(N).vert(); 
    N.back(); 
    F.movedBy(N).vert(); 
    B.movedBy(N).vert();  
    endShape();
  };
  
  void paintLine() {
    B.showLineTo(F); 
  };
  
  boolean selected() {
    pickedF = F.vecTo(mouse()).norm()<30; 
    pickedB = B.vecTo(mouse()).norm()<30; 
    picked=pickedF||pickedB; 
    return(picked);
  }
  
  void track() {
    pt M=mouse(); 
    if (pickedF) {
      F.setTo(M);
    } 
    if (pickedB) {
      B.setTo(M);
    }
  }
  
  void paintDots() {
    if (!pickedB) {
      fill(yellow);
      B.show(20);
    };  
    if (!pickedF) {
      fill(yellow); 
      F.show(20); 
    }; 
  };
  
  void snap() {
    if (pickedB) {
      if(abs(B.x-F.x)<5) {
        B.x=F.x;
      }; 
      if(abs(B.y-F.y)<5) {
        B.y=F.y;
      }; 
    }; 
    if (pickedF) {
      if(abs(F.x-B.x)<5) {
        F.x=B.x;
      }; 
      if(abs(F.y-B.y)<5) {
        F.y=B.y;
      }; 
    }; 
  }
}

class car {
  float h=40, w=100;                        // car dimensions
  color col;                               // color of car
  pt B=new pt(0,0), F=new pt(w,0);         // front and back points
  float a=0.0;                     // angle and increment
  boolean picked = false;                  // true if being dragged
  
  int numPts = 4;
  pt[] animPts = new pt[numPts];   // array of points that control the path the car follows when animating
  int animPicked = -1;   // -1 if there is no control point being dragged, otherwise # is selected index
  
  pt B2, F2;
  float a2;
  
  car (float px, float py, float pa, color pc) {
    B.x=px; 
    B.y=py; 
    a=pa; 
    col=pc; 
    F.x=px+w; 
    F.y=py; 
    F.turnAround(B,-a);
    
    for (int i = 0; i < numPts; i++) {
      vec distVec = B.vecTo(F);
      distVec.mul((float)i);
      
      pt tempPt = F.make();
      tempPt.addVec(distVec);
      animPts[i] = tempPt;
    }      
  }     // creation
    
  void paint() {
    pushMatrix(); 
    translate(B.x,B.y); 
    rotate(a);              // paints car
    noStroke();
    if (picked) {
      fill(yellow);
    } else {
      fill(col);
    }; 
    rect(w/2,0,w,h); 
    ellipse(0,0,w/7,h*0.95); 
    ellipse(w,0,w/5,h*0.95);      
    fill(240); 
    ellipse(w*0.2,0,w*0.3,h); 
    ellipse(w*0.2+w/3,0,w*0.4,h);
    if (picked) {
      fill(yellow);
    } else {
        fill(col);
    };
    rect(w*0.2+w/6,0,2*w/6,h);    
    popMatrix(); 
  }
  
  boolean selected() {
    boolean pic = F.vecTo(mouse()).norm()<w/5;
    picked=pic;
    return(pic); 
  }
  
  void write() {
    println("Car: a="+a); 
    print(" B="); 
    B.write(); 
    print(" F="); 
    F.write(); 
  }
  
  void track() {
    pt M=mouse();   
    vec FB = F.vecTo(B); 
    float fb = FB.norm(); 
    vec MB = M.vecTo(B); 
    float mb = MB.norm();
    
    if (mb<0.0001) {
      F.setTo(M);
      B.addVec(FB);
    } else {
      F.setTo(M); 
      M.addScaledVec(fb/mb,MB); 
      B.setTo(M); 
    };
    a=angle(I,B.vecTo(F));
    
    animPts[0] = F.make();
  }
 
  void paintDots() {
    if (!picked) { 
      F.show(20); 
    }; 
  };
  

  /** method checks to see whether one of the vehicle control points has been selected
       (returns the index of the selected point if true, -1 if no point selected) **/
  int controlPtSelected() {
     for (int i = 0; i < numPts; i++) {
       if (animPts[i].vecTo(mouse()).norm()<4) {
         return i;   // return index of selected point
       }
     }
     return -1;   // return -1 if nothing found
  }
}     
 
void setup() { 
  size(1000, 775);
  createCars();
  createStreets(); }

void createCars () {
   C[0] = new car(760,300,2*PI/3,red);  
   C[1] = new car(690,520,3*PI/2,green);  
/*   C[2] = new car(780,500,3*PI/2,blue); 
   C[3] = new car(450,310,0,grey); 
   C[4] = new car(940,220,PI,grey); 
   C[5] = new car(600,60,PI/2,grey); */
    }
   
void createStreets () {
   S[0] = new street(650,50,650,750,90);
   S[1] = new street(740,50,740,750,90);
   S[2] = new street(50,270,950,270,90);
   S[3] = new street(200,50,200,750,90);
/*   S[4] = new street(310,620,440,450,70);
   S[5] = new street(290,610,460,720,70);*/
    }
   
void draw()   {background (255);  rectMode(CENTER);  
  for (int i=0; i<ns; i++) {
    S[i].paint(); 
  }; 
  stroke(255,245,200);  
  for (int i=0; i<ns; i++) {
    S[i].paintLine(); 
  }; 
  if (!animating) {
    for (int i=0; i<nr; i++) {
      if (C[i].picked) {  
        fill(200,100,100);
      } else {
        fill(100,200,100);
      };
      C[i].paint(); 
    }; 
    if (keyPressed&&(key==' ')) {
      for (int i=0; i<ns; i++) {
        S[i].paintDots();
      };  
      for (int i=0; i<nr; i++) {
        C[i].paintDots();
      };
    };
  
    if (displayPath) {      // code for displaying the control path of the cars
      strokeWeight(1);
      stroke(lightGrey);
      noFill();
      for (int i=0; i < nr; i++) {
        for (int j=0; j < C[i].numPts - 1; j++) {
          ellipse(C[i].animPts[j].x, C[i].animPts[j].y, 5, 5);
          line (C[i].animPts[j].x, C[i].animPts[j].y, C[i].animPts[j+1].x, C[i].animPts[j+1].y);
          ellipse(C[i].animPts[j+1].x, C[i].animPts[j+1].y, 5, 5);
        }
      } 
    }
  } else {
    t += 0.04;
    float s = t / (C[0].numPts - 1);  // adjust t to [0, 1]
    if (s > 1) {          // allowing one "second" per control point
      for (int i = 0; i < nr; i++) {
        C[i].B = C[i].B2.make();
        C[i].F = C[i].F2.make();
        C[i].a = C[i].a2;
      }
      animating = false;
    } else {
      for (int i = 0; i < nr; i++) {
        pt[] backupPts = new pt[4];
        for (int k = 0; k < C[i].numPts; k++) backupPts[k] = C[i].animPts[k].make();
        
        C[i].animPts[3].interpolate(C[i].animPts[2],s,C[i].animPts[3]);    //slide(E) = E1
        C[i].animPts[2].interpolate(C[i].animPts[1],s,C[i].animPts[2]);    //slide(D) = D1
        C[i].animPts[1].interpolate(C[i].animPts[0],s,C[i].animPts[1]);    //slide(B) = B1
        C[i].animPts[3].interpolate(C[i].animPts[2],s,C[i].animPts[3]);    //slide(E) = E2
        C[i].animPts[2].interpolate(C[i].animPts[1],s,C[i].animPts[2]);    //slide(D) = D2
        
        vec tanVec = C[i].animPts[3].vecTo(C[i].animPts[2]);
        
        C[i].animPts[3].interpolate(C[i].animPts[2],s,C[i].animPts[3]);    //slide(E) = C
        
        C[i].F = C[i].animPts[3].make();
        

        C[i].B = C[i].F.make();
        C[i].B.addVec(new vec(C[i].w, 0));
        C[i].B.turnAround(C[i].F, 2 * PI - angle(new vec(1.0, 0.0), tanVec));
        C[i].a = angle(new vec(1.0, 0.0), C[i].B.vecTo(C[i].F));
        
        C[i].paint();
        for (int k = 0; k < C[i].numPts; k++) C[i].animPts[k] = backupPts[k].make();
      }
    }
  }
}
    
void mousePressed() {
  boolean found=false; 
  for (int i=nr-1; i>=0; i--) { 
    if (!found) {
      if(C[i].selected()) { 
        found=true;
      };
      if (displayPath) {                         // don't allow pts to be moved if not visible
        int ctrlPt = C[i].controlPtSelected();   
        if(ctrlPt != -1) {                       
          C[i].animPicked = ctrlPt;
          found=true;
        };
      };
    }; 
  };
  
  for (int i=ns-1; i>=0; i--) { 
    if (!found) {
      if(S[i].selected()) { 
        found=true;
      }; 
    }; 
  };
} 

void mouseDragged() { 
  for (int i=0; i<nr; i++) { 
    if(C[i].picked) {
      C[i].track(); 
    }; 
    if(C[i].animPicked != -1) {
      C[i].animPts[C[i].animPicked] = mouse().make();
    };
  };
  for (int i=0; i<ns; i++) { 
    if(S[i].picked) {
      S[i].track(); 
      S[i].snap();
    };
  };
}

void mouseReleased() {
  for (int i=0; i<nr; i++) {
    C[i].picked=false;
    C[i].animPicked = -1;
  }; 
  for (int i=0; i<ns; i++) {
    S[i].pickedF=false;
    S[i].pickedB=false;
    S[i].picked=false;
  }; 
};

void keyPressed() { 
  if (key=='c') {
    createCars();
  }; 
  if (key=='s') {
    createStreets();
  };
  if (key=='p') {
    displayPath = !displayPath;  // 'p' toggles whether the control points for vehicles are displayed or not
  };
  if (key=='a') {
    t = 0.0;
    for (int i = 0; i < nr; i++) {
      C[i].B2 = C[i].B.make(); 
      C[i].F2 = C[i].F.make();
      C[i].a2 = C[i].a;
    }
    animating = true;                   // Animation of accident
  };
  if (key=='f') {
                       // Animation of how accident could have been avoided by incorrect driver
  };
  if (key=='d') {
                       // Animation of how accident could have been avoided by defensive driver
  };
  if (key=='S') {
                       // Saves the accident scene 
  }
}