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
  pt Br=new pt(0,0), Fr=new pt(w,0);
  float a=0.0;                              // angle and increment
  boolean picked = false;                   // true if being dragged
  
  //====================================
  float collisionDiameter = 50;
  pt collisionDisk1 = new pt(3*w/4, 0), collisionDisk2 = new pt(w/4, 0);
  CubicBezier[] path;
//  QuadraticBezier speed;
  //====================================
  
  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);
    Br.setTo(B);
    Fr.setTo(F);
    
    collisionDisk1.x = B.x + 3*w/4;
    collisionDisk1.y = B.y;
    collisionDisk1.turnAround(B,-a);
    collisionDisk2.x = B.x + w/4;
    collisionDisk2.y = B.y;
    collisionDisk2.turnAround(B,-a);
        
    path = new CubicBezier[3];
    for(int k = 0; k < 3; k++)
      path[k] = null;
  }     // creation
  
  void paint(boolean makingPath)
  {
    pushMatrix();
    translate(B.x,B.y);
    rotate(a);              // paints car
    noStroke();
    //====================================
    if(makingPath)
      fill(0,0,0);
    else
    //====================================
    if (picked)
    {
      fill(yellow);
    }
    else
    {
      fill(col);
    };
    
    specular(255);
    
    translate(w/2, 0, h/4);
    box(w, h, h/2);
    translate(-w/2, 0, -h/4);
    
    translate(w/3, 0, 3*h/4);
    box(2*w/3, h, h/2);
    translate(-w/3, 0, -3*h/4);
    
    fill(180, 180, 180);
    translate(2*w/3, 0, 2*h/5);
    sphere(h/2);
    translate(-2*w/3, 0, -2*h/5);
    
    fill(255, 255, 255);
    emissive(255);
    translate(w, h/3, h/3);
    sphere(h/12);
    translate(-w, -h/3, -h/3);
    translate(w, -h/3, h/3);
    sphere(h/12);
    translate(-w, h/3, -h/3);
    emissive(0);
    
    specular(0);
    
    /*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();
    
    //====================================
    if(mode > -1)
    {
      //Draw collision disks
      noFill();
      stroke(0, 0, 0);
      ellipse(collisionDisk1.x, collisionDisk1.y, collisionDiameter, collisionDiameter);
      ellipse(collisionDisk2.x, collisionDisk2.y, collisionDiameter, collisionDiameter);
      
      if(path[mode] != null)
      {
        //Draw control points
        fill(0);
        ellipse(path[mode].p0.x, path[mode].p0.y, 7, 7);
        ellipse(path[mode].p1.x, path[mode].p1.y, 7, 7);
        ellipse(path[mode].p2.x, path[mode].p2.y, 7, 7);
        ellipse(path[mode].p3.x, path[mode].p3.y, 7, 7);
      
        //Draw control polygon
        stroke(255, 0, 0);
        line(path[mode].p0.x, path[mode].p0.y, path[mode].p1.x, path[mode].p1.y);
        line(path[mode].p1.x, path[mode].p1.y, path[mode].p2.x, path[mode].p2.y);
        line(path[mode].p2.x, path[mode].p2.y, path[mode].p3.x, path[mode].p3.y);
        line(path[mode].p3.x, path[mode].p3.y, path[mode].p0.x, path[mode].p0.y);
      
        //Draw Bezier curve
        stroke(0, 255, 0);
        bezier(path[mode].p0.x, path[mode].p0.y,
              path[mode].p1.x, path[mode].p1.y,
              path[mode].p2.x, path[mode].p2.y,
              path[mode].p3.x, path[mode].p3.y);
      }
    }
    //====================================
  }
  
  boolean selected()
  {
    boolean pic = F.vecTo(mouse()).norm()<w/5;
    picked=pic;
    return(pic);
  }
  
  boolean selected2()
  {
    return F.vecTo(mouse()).norm()<w/5;
  }
  
  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)); 
    updateCollisionDisks();
  }
  
  void paintDots()
  {
    if (!picked)
    {
      F.show(20);
     };
  };
  
  void setPath(CubicBezier _path, int index)
  {
    if(index > -1)
      path[index] = _path;
    else
      println("ERROR IN car.setPath");
  }
  
  void animate(float t, int animate_mode)
  {
    if(animate_mode > -1)
    {
      if(path[animate_mode] != null)
      {
        pt M = path[animate_mode].evaluate(t);
        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));
        updateCollisionDisks();
      };
    }
  }
  
  void updateCollisionDisks()
  {
    collisionDisk1.x = B.x + 3*w/4;
    collisionDisk1.y = B.y;
    collisionDisk1.turnAround(B,-a);
    collisionDisk2.x = B.x + w/4;
    collisionDisk2.y = B.y;
    collisionDisk2.turnAround(B,-a);
  }
  
  String toString() {   
    String list = str(B.x) + " " + str(B.y) + " " + str(a) + " " + str(red(col)) + " " + str(green(col)) + " " + str(blue(col));
    if(path[0] != null) {
      list = list + " " + str(1) + " " + str(path[0].p0.x) + " " + str(path[0].p0.y) + " " + str(path[0].p1.x) + " " + str(path[0].p1.y) + " " + str(path[0].p2.x) +
      " " + str(path[0].p2.y) + " " + str(path[0].p3.x) + " " + str(path[0].p3.y);
    }
    else {
      list = list + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0);
    }
    if(path[1] != null) {
      list = list + " " + str(1) + " " + str(path[1].p0.x) + " " + str(path[1].p0.y) + " " + str(path[1].p1.x) + " " + str(path[1].p1.y) + " " + str(path[1].p2.x) +
      " " + str(path[1].p2.y) + " " + str(path[1].p3.x) + " " + str(path[1].p3.y);
    }
    else {
      list = list + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0);
    }
    if(path[2] != null) {
      list = list + " " + str(1) + " " + str(path[2].p0.x) + " " + str(path[2].p0.y) + " " + str(path[2].p1.x) + " " + str(path[2].p1.y) + " " + str(path[2].p2.x) +
      " " + str(path[2].p2.y) + " " + str(path[2].p3.x) + " " + str(path[2].p3.y);
    }
    else {
      list = list + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0) + " " + str(0);
    }
    //println(unhex(hex(col, 6)));
    return list;
  }
}

//===============================================
pt checkForCollision(car M, car N)
{
  pt answer = circleCollision(M.collisionDisk1, N.collisionDisk1, M.collisionDiameter);
  if(answer != null)
    return answer;
    
  answer = circleCollision(M.collisionDisk2, N.collisionDisk1, M.collisionDiameter);
  if(answer != null)
    return answer;
    
  answer = circleCollision(M.collisionDisk1, N.collisionDisk2, M.collisionDiameter);
  if(answer != null)
    return answer;
    
  answer = circleCollision(M.collisionDisk2, N.collisionDisk2, M.collisionDiameter);
  if(answer != null)
    return answer;
  
  return null;
}

pt circleCollision(pt P1, pt P2, float d)
{
  vec v = P1.vecTo(P2);
  if(v.norm() > d)
    return null;
    
  v.unit();
  v.mul(d / 2);
  pt collisionPoint = P1.make();
  collisionPoint.addVec(v);
  return collisionPoint;
}