import processing.opengl.*;
//*************************
//*    crazy cars demo of naive dynamics with collisions
//*    written by Jarek Rossignac in July 2006
//*
//*******************************************************/
color red =  color(255,0,0), green =color(0,255,0), yellow =color(205,205,0), grey =color(205,205,205), blue =color(0,0,255);    // colors R,G,B
float _friction=0.02, _shockLoss=0.99;                      // speed reduction due to frictino and in wall collision
vec labelD=new vec(-3,2);                                    // offset for car labels
int _nBalls=20; BALL B[] = new BALL [_nBalls];              // number of balls (2 per car) and their array
pt _mousePic = new pt(0,0); pt _mouseNow = new pt(0,0);     // where mouse was when pressed and where it is now
int _picked=-1;                                            // which ball was selected on mouse press (-1 if none)
// float _lastTime=millis(), _currentTime=millis(), _deltaTime=_currentTime-_lastTime;  // keep track of time and frame duration
boolean step=false;                                        // for sep by step simulation (when pressing 'z'
//  PImage carImage = loadImage("car2.jpg");
//PImage carImage = loadImage("car2.jpg");
int ncars=3;
PImage carImage[] = new PImage[ncars];

void setup(){ 
    // size(600, 600);                              // init with a window of 600x600
   size(600, 600, OPENGL);                              // init with a window of 600x600
   PFont font = loadFont("Courier-14.vlw"); textFont(font, 12);  // you MUST load font for labels (Press Tools > Load Fonts... > pick curier size 14    
   carImage[0] = loadImage("car0.jpg");
   carImage[1] = loadImage("car1.jpg");
   carImage[2] = loadImage("car2.jpg");

   for(int i=0; i<_nBalls; i+=2) {                        // initial position of balls
//      B[i]=new BALL();  B[i].C.x=random(B[i].r,width-B[i].r); B[i].C.y=random(B[i].r,height-B[i].r);   B[i].V.setTo(4*B[i].r,0);  B[i].angle=random(-PI/3,PI/3);
      B[i]=new BALL();            // create ball (front of car)
      B[i].C.x=2*(i+1)*B[i].r;   // compute the x coord of its center (C) in terms of i and its radius (r)
      B[i].C.y=height-3*B[i].r;  // put the cars near the lower edge (y goes down) 
      B[i].V.setTo(4*B[i].r,0);  // set the velocity vector to te 4r forward
      B[i].angle=random(-PI/3,PI/3);  // set the default angle of speed direction to body orientation 
      B[i+1]=new BALL();         // nxt ball is the rear of car
      B[i+1].C.setTo(B[i].C);    // same x as the front ball
      B[i+1].C.y+=2*B[i+1].r;   // 2r below the front ball
      B[i+1].V.setTo(2*B[i+1].r,0); // no velocity
      B[i+1].angle=0;            // 0 default angle (if it gains speed)
      turnV(B[i+1],B[i]);        // (optional) turn velocity vectors to their defaule angle (0) 
      }; 
   setColors();                  // reset the natural colors
   }
void setColors () {for(int i=0; i<_nBalls; i++) {if(i%2==0) {B[i].col=yellow;} else {B[i].col=red;}; };}

void draw()   {                           // executed at each frame
    background (255,255,255); smooth();  // clear screen and zbuffer, smooth drawing mode for edges
    textureMode(NORMALIZED);
    _mouseNow.setToMouse();                // locate the mouse
      if (mousePressed&&(_picked!=-1) ) {    // when the mouse is still down and a car was picked
        if (keyPressed&&(key==CODED)) {      // if shift or CTRL keys are pressed, we edit the velocity
          B[_picked].setVtoMouse();         // set velocity vector to be from ball to mouse
          int i=_picked; if (i%2==0) {        // tests whether front or back of car was picked
            B[i].angle=0;  turnV(B[i+1],B[i]);  // (optional) if front, force the velocity to be forward
            lockAngle(B[i+1],B[i]);            // lock angle 
            } 
          else {                          // if back was picked
            lockAngle(B[i],B[i-1]);
            };  
          }
        else {                           // no shift or CTRL                       
          B[_picked].snapTo(_mouseNow);     // drag the picked ball
          int o=_picked+1; if (o%2==0) {o=_picked-1;};  // the other ball of the car
          B[o].moveTowardsBall(1.0,B[_picked]); // drag it along so that it stays in contact 
          };
        };    
   if ((!keyPressed&&step)||(keyPressed&&(key==' '))) { // execute next animation step when SPACE is down or in step mode ('z' was pressed and released
     for(int i=0; i<_nBalls; i++) {B[i].move();};        // move all balls using their velocities
     for(int i=0; i<_nBalls; i++) {B[i].bounceOffWalls();};  // process collisions with walls
     for(int i=0; i<_nBalls; i++) {for(int j=i+1; j<_nBalls; j++) {  // for all pairs of balls
        if (clash(B[i],B[j])) {          // if they collide
          if ((i%2==1)||(j!=i+1)) {  // and are not in the same car
            B[i].recover=100; B[j].recover=100;  // it will take them 100 frames to recover from collision
            }; 
          shock(B[i],B[j]);         // process shock to get bouced-off velocities
          converge(B[i],B[j]);     // move them so that they are not penetrating but touching
          };
        }; 
      };
//     for(int i=0; i<_nBalls; i++) {B[i].pinL();};            // remember positions
     for(int i=0; i<_nBalls; i+=2) {converge(B[i],B[i+1]);};  // Bring the two balls in contact
//     for(int i=0; i<_nBalls; i++) {B[i].setV();};          // set velocities from remembered position to current
     for(int i=0; i<_nBalls; i++) {B[i].dampV(); };          // reduce velocities if sliding
      };
   for(int i=0; i<_nBalls; i+=2) { turnV(B[i+1],B[i]);};    // adjusts V towards the default angle
//   stroke(10); for(int i=0; i<_nBalls; i++) { fill(B[i].col); B[i].show();};  / renders cars as 2 balls and a square
   stroke(10); for(int i=0; i<_nBalls; i+=2) { B[i].showV(); };    // shows the V vector
   fill(255); noStroke(); for(int i=0; i<_nBalls; i+=2) {  drawRectangleTextured(B[i],B[i+1],i%3); };  // renders textured images over cars
   fill(255); for(int i=0; i<_nBalls; i+=2) {B[i].C.label(str(int(i/2)),labelD); };     // print label on car
   };

void mousePressed() {_mousePic.setToMouse(); _picked=-1; for(int i=0; i<_nBalls; i++)  {if((_picked==-1)&&(B[i].selected())) {_picked=i;}; };} 
void mouseReleased() {_picked=-1;}
void keyPressed() {
  if (key=='z') {step=true;};  
  if (key=='w') {for(int i=0; i<_nBalls; i++) {B[i].write();};};  
 }
 
void drawRectangle(BALL B1, BALL B2) {
  pt V= new pt(0,0); 
  vec T=B1.C.vecTo(B2.C).left(); T.unit();
  beginShape(); 
  V.setTo(B1.C); V.addScaledVec(B1.r,T); V.vert();
  V.setTo(B2.C); V.addScaledVec(B2.r,T); V.vert();
  V.setTo(B2.C); V.addScaledVec(-B2.r,T); V.vert();
  V.setTo(B1.C); V.addScaledVec(-B1.r,T); V.vert();
  endShape();
  }; 
  
void drawRectangleTextured(BALL B1, BALL B2,int pic) {
  pt V= new pt(0,0); 
  vec N = B1.C.vecTo(B2.C); N.unit();   vec T=N.left();
  beginShape(QUADS);  
  texture(carImage[pic]);
  V.setTo(B1.C);  V.addScaledVec(-B1.r,N); V.addScaledVec(B1.r,T);  vertex(V.x,V.y,0,0);
  V.setTo(B2.C); V.addScaledVec(B1.r,N); V.addScaledVec(B2.r,T);  vertex(V.x,V.y,1,0);
  V.setTo(B2.C); V.addScaledVec(B1.r,N); V.addScaledVec(-B2.r,T); vertex(V.x,V.y,1,1);
  V.setTo(B1.C); V.addScaledVec(-B1.r,N); V.addScaledVec(-B1.r,T); vertex(V.x,V.y,0,1);
  endShape();
  }; 

void lockAngle(BALL B1, BALL B2) {vec N = B1.C.vecTo(B2.C); B1.angle=angle(N,B1.V); B2.angle=angle(N,B2.V); }; 
void turnV(BALL B1, BALL B2) {vec N = B1.C.vecTo(B2.C); float a1=angle(N,B1.V); float a2=angle(N,B2.V); B1.V.turn((B1.angle-a1)/B1.recover); B2.V.turn((B2.angle-a2)/B2.recover);}; 
void converge(BALL B1, BALL B2) {pt M=midPt(B1.C,B2.C); B1.flushTo(M); B2.flushTo(M);}; 
boolean clash(BALL B1, BALL B2) { return(B1.C.disTo(B2.C)<B1.r+B2.r); };
void shock(BALL B1, BALL B2) {
  vec N = B1.C.vecTo(B2.C); N.unit(); 
  float v1=dot(B1.V,N); float v2=dot(B2.V,N); 
  B1.V.addScaled(v2-v1,N); B2.V.addScaled(v1-v2,N); 
  };

class BALL {pt C=new pt(width/2,height/2); vec V=new vec(0,0); float r=15; color col=yellow; pt L=new pt(width/2,height/2); float angle=0; int recover=1;
   BALL () {};
   void write() {print   ("angle="+(angle*180/PI)+", C="); C.write();}; 
   void show() { if (recover==1) {fill(col);} else {fill(blue);}; C.show(r);}; 
   void showV() {V.show(C);}; 
   void move() {
     C.x=C.x+V.x/10; C.y=C.y+V.y/10; 
     if (recover>1) {recover--; angle*=0.9;}; 
     float n=V.norm(); 
     if ((recover==1)&&(n<10)) {
       if (n>0.01) {V.mul((n+_friction)/n); angle+=random(-PI/60,PI/60); angle*=0.97; }
       else {V.setTo(50,0); angle=PI+random(-PI/2,PI/2); recover=20;};  
       };
   };
   void pinL() {L.setTo(C);};
   void setV() {V.add(L.vecTo(C));}; 
   void resetL() {L.setTo(C); L.subVec(V);};
   boolean selected() {boolean pic = C.vecTo(_mousePic).norm()<r; return(pic); }
   void snapTo(pt P) {C.setTo(P);}
   void flushTo(pt P) {float n = C.disTo(P); C.moveTowards((n-r)/n,P);}
   void setVtoMouse() {V.setTo(C.vecTo(_mouseNow)); }; 
   void moveTowardsBall(float d, BALL O) {float dBO = C.vecTo(O.C).norm(); float dd=dBO-2*r; C.moveTowards(d*dd/dBO,O.C);};
   void dampV() { if (recover!=1) { float n=V.norm(); if (n>_friction) {V.mul((n-_friction)/n); } else {V.setTo(0,0);};}; };
   void bounceOffWalls (){
      if (C.x<r) {C.x=2*r-C.x; V.x=-V.x; V.x=V.x*_shockLoss; recover=100;}; if (C.x>width-r) {C.x=2 * (width-r)-C.x; V.x=-V.x; V.x=V.x*_shockLoss;recover=100;}; 
      if (C.y<r) {C.y=2*r-C.y; V.y=-V.y; V.y=V.y*_shockLoss;recover=100;}; if (C.y>height-r) {C.y=2 * (height-r)-C.y; V.y=-V.y; V.y=V.y*_shockLoss;recover=100;};
       };
   void bounceOffFixedBall(BALL O) { 
      if (C.vecTo(O.C).norm() < r+O.r)    {
         vec T = V.make(); T.unit();   
         vec N = T.left(); 
        float h = dot(C.vecTo(O.C),N); float w = sqrt(sq(r+O.r)-sq(h));
        T.mul(-w);  N.mul(-h);  T.add(N);       
        T.unit(); T.mul(-2*dot(T,V));   V.add(T);  
        }; 
      };
  void follow (BALL O) { 
     pt B=O.C.make();                        //B.show(3);
     pt CC=B.make(); CC.addVec(O.V);         //CC.show(3); 
     vec N = C.vecTo(O.C); 
     pt A=C.make(); A.addScaledVec(-1.0,N);   // A.show(3); 
     pt G = circleCenter(A,B,CC);             // G.show(4);
     float a= angle(G.vecTo(B),G.vecTo(CC));  
     vec T = G.vecTo(C); 
     T.turn(a);                     
     G.addVec(T);
     V.setTo(C.vecTo(G));                        V.show(C);
    };
   }      // end of class BALL

pt circleCenter (pt A, pt B, pt C) {    // computes the center of a circumscirbing circle to triangle (A,B,C)
  vec AB =  A.vecTo(B);  float ab2 = dot(AB,AB);
  vec AC =  A.vecTo(C); AC=AC.left();  float ac2 = dot(AC,AC);
  float d = 2*dot(AB,AC);
  AB=AB.left();
  AB.mul(-ac2);
  AB.addScaled(ab2,AC);
  pt X =  A.make();
  X.addScaledVec(1.0/d,AB);
  return(X);
  };