class CarPose
{
  CarPose ( pt point, pt direction ) { 
    p = point; 
    dir = direction; 
  }

  void saveTo (ArrayList a)
  {
    p.saveTo(a);
    dir.saveTo(a);
  }

  void readFrom (ArrayList a)
  {
    p = new pt(0,0);
    p.readFrom(a);
    dir = new pt(0,0);
    dir.readFrom(a);
  }

  pt p;
  pt dir; 
}

class Car
{
  BezierCurve bc = null;
  DistanceCurve dc = null;
  ArrayList poses = new ArrayList();
  int id;
  int curControlPoint=-1, curAnchorPoint=-1;
  pt constrainPt = new pt(0,0);
  pt offsetPt = new pt(0,0);
  pt offsetDir = new pt(0,0);
  int bordersize = 16;
  pt[] border = new pt[bordersize];

  boolean curveControlled = true;
  pt      curpos = new pt(0,0);
  pt      curvelo = new pt(0,0);
  float   curangle = 0;
  float   curanglevelo = 0;
  float   lastTime = 0;
  pt      tmppos = new pt(0,0);
  float   tmpangle;

  void  saveTo(ArrayList a)
  {
    a.add ( "" + id );
    bc.saveTo(a);
    dc.saveTo(a); 

    a.add ("" + poses.size());
    for ( int i =0; i < poses.size(); ++i)
      ((CarPose) poses.get(i)).saveTo(a);

  }

  void readFrom (ArrayList a)
  {
    id = Integer.parseInt ( (String) a.remove(0) );
    bc = new BezierCurve ();
    bc.readFrom(a);
    dc = new DistanceCurve(bc);
    dc.readFrom(a);

    int n= Integer.parseInt ( (String) a.remove(0) );
    for ( int i =0; i < n; ++i) {
      CarPose pose = new CarPose (new pt(0,0), new pt(0,0));
      pose.readFrom(a);
      poses.add(pose);
    }    

  }

  Car(int i)
  {
    id = i;

    float w = carSprite.width;
    float h = carSprite.height;

    border[0] = new pt ( w/24.0, -h/12 );
    border[1] = new pt ( w/24.0*23.0, -h/12 );
    border[2] = new pt ( w, -h/24.0 );
    border[3] = new pt ( (1.0/36.0 + 1)*w, 0 );
    border[4] = new pt ( w/24.0*25.0, h/12.0 );
    border[5] = new pt ( w/24.0*25.0, h/12.0 * 11.0 );
    border[6] = new pt ( (1.0/36.0 + 1)*w, h );
    border[7] = new pt ( w, h* (1.0/24.0+1.0 ) );
    border[8] = new pt ( w/24.0*23.0, h*13.0/12.0 );
    border[9] = new pt ( w/24.0, h*13.0/12.0 );
    border[10] = new pt ( 0, h* (1.0/24.0+1.0 ) );
    border[11] = new pt ( -1.0/36.0*w, h );
    border[12] = new pt ( -w/24.0, h/12.0 * 11.0 );
    border[13] = new pt ( -w/24.0, h/12.0 );
    border[14] = new pt ( -1.0/36.0*w, 0 );
    border[15] = new pt ( 0, -h/24.0 );

  }

  void drawSpeedCurve (boolean midPoint)
  {
    if ( dc != null) {
      dc.drawCurve(midPoint); 
    }
  }

  void rebuildSpeedCurve ()
  {
    dc = new DistanceCurve (bc);
  }

  void updateBezier (boolean updateSpeed)
  {
    bc = new BezierCurve(poses);
    if (updateSpeed)
      rebuildSpeedCurve ();
  }

  void updateVelocityFromCurvature()
  {
    bc.updateVelocityFromCurvature (); 
  }

  void anim (boolean normalAnim)
  { 
    pt val = new pt(0,0);
    pt tangent = new pt(0,0);

    if (normalAnim)
    {
      drawCar (curpos,curangle,.55);
    }
    else
    {
      if ( dc != null && dc.curControlPoint >= 0 ) 
      {
        float distance =  ((BezierControlPoint)dc.pts.get(dc.curControlPoint)).p.y ;
        bc.sampleAt (distance,val,tangent);
        drawCar (val,atan2(tangent.y, tangent.x), 0.4);
      }
    }
  }

  void drawPositions ()
  {
    for (int i=0, sz = poses.size(); i < sz; ++i )
    {
      CarPose pose = (CarPose) poses.get(i);
      drawCar (pose.p, atan2(pose.dir.y, pose.dir.x),0.3);
      colorMode (HSB,1.0);
      float col = (id/15.0)+0.5;
      if (col > 1 )
        col -= 1.0;

      stroke (col, 1.0, 1.0, 1.0);
      strokeWeight (3);
      line ( pose.p.x, pose.p.y, pose.p.x + pose.dir.x, pose.p.y+pose.dir.y); 
      fill  (col,1.0,1.0,1.0);
      ellipse ( pose.p.x + pose.dir.x, pose.p.y+pose.dir.y,3,3);
      colorMode (RGB,255);
      strokeWeight(1);

      fill(255);
      stroke(0);
      pushMatrix();

      translate (pose.p.x, pose.p.y);
      // car is flipped
      rotate ( atan2 (pose.dir.y, pose.dir.x) + PI);
      translate (-carSprite.width/2, -carSprite.height/2);
      ellipse (6,carSprite.height/2,12,12);
      stroke(0);
      fill(0);
      translate (5,carSprite.height/2);
      rotate (- atan2 (pose.dir.y, pose.dir.x) - PI);
      translate (-5,-carSprite.height/2);
      textAlign (CENTER);
      textFont(standardFont,10);
      text ( "" +i,5,carSprite.height/2+5);
      popMatrix();
    }
  }

  void drawCar (pt val, float angle, float alpha)
  {
    pushMatrix();

    translate (val.x, val.y);
    // car is flipped
    rotate ( angle + PI);

    translate (-carSprite.width/2, -carSprite.height/2);
    colorMode (HSB,1.0);
    tint ( id/15.0, 1.0,1.0,alpha );
    image (carSprite,0,0);
    popMatrix();

    float w = carSprite.width;
    float h = carSprite.height;

    pushMatrix ();
    translate (val.x, val.y);
    rotate ( angle );    
    translate (-w/2, -h/2);
    stroke ( id/15.0, 1.0,1.0,alpha );
    fill ( id/15.0, 1.0,1.0,alpha );

    beginShape(POLYGON);
    for ( int i =0; i < bordersize; ++i )
    {
      border[i].vert();
    }
    endShape();

    noTint ();
    colorMode (RGB,255);

    popMatrix();


  }
  
  /*
  void draw3DCar (boolean boxOnly)
  {
    float w = carSprite.width;
    float h = carSprite.height;

    float maxx = d3engine.obj.maxx;
    float maxz = d3engine.obj.maxz;
    float miny = d3engine.obj.miny;
    float maxy = d3engine.obj.maxy;
    float minx = d3engine.obj.minx;
    float minz = d3engine.obj.minz;     

    float sw = w / (maxz-minz);
    float sh = h / (maxx-minx);

    pushMatrix();
    translate(curpos.x,-2,-(botrightCurve.y-curpos.y));
    rotateY(PI/2-curangle);
    scale ( sw,(sw+sh)*0.5,sh);
    translate ( - (maxx + minx) * 0.5, 0, -(maxz+minz)*0.5 );
    if (!boxOnly)
      d3engine.drawCar(id);
    else
    {
      colorMode (HSB,1.0);
      fill ( id/15.0, 1.0,1.0);
      
      translate ((minx+maxx)*0.5, (miny+maxy)*0.5, (minz+maxz)*0.5);
      box ( maxx-minx, maxy-miny, maxz-minz );

      colorMode(RGB,1.0);
    }
    popMatrix();  
  }
  */

  void detachFromCurve(float t)
  {
    float h = 1e-4;
    updatePosFromTime(t-h);
    pt lval = curpos.makeCopy();
    float langle = curangle;
    updatePosFromTime(t+h);
    curvelo.setFromPt ( curpos.sub(lval).sca(0.5/h) );
    curanglevelo = (curangle-langle)*0.5/h;
    updatePosFromTime(t);
    curveControlled = false; 
  } 

  pt center ()
  {
    pt center = new pt(0,0);
    center.rotate ( curangle );
    center.addPt (curpos);
    return center;
  }

  void resetToCurve()
  {
    curveControlled = true;
    lastTime =0;
  }


  pt[] getCurrentBorder ()
  {
    pt[] res = new pt[bordersize];
    pt offset = new pt ( -carSprite.width/2, -carSprite.height/2 );

    for (int i =0; i < bordersize; ++i )
    {
      pt cur = border[i].makeCopy();
      cur.addPt (offset);
      cur.rotate ( tmpangle );
      cur.addPt (tmppos);
      res[i] = cur;
    }
    return res; 
  }

  pt[] getCurrentBorderWithRect (pt upleft, pt botright)
  {
    pt res[] = getCurrentBorder();
    upleft.setFromPt(res[0]);
    botright.setFromPt(res[0]);

    for ( int i = 1; i < bordersize; ++i )  
    {
      upleft.x = min( upleft.x, res[i].x);
      upleft.y = min( upleft.y, res[i].y);
      botright.x = max( botright.x, res[i].x);
      botright.y = max( botright.y, res[i].y);
    }

    return res;
  }

  void drawCurve (boolean midPoint)
  {
    bc.drawCurve(midPoint); 
  }

  boolean handlePressEvent ()
  {
    //check if car is selected
    for ( int i=0, sz = poses.size(); i < sz; ++i )
    {
      CarPose pose = (CarPose) poses.get(i); 
      pt anchor = new pt(8,carSprite.height/2);
      anchor.subPt ( new pt ( carSprite.width/2, carSprite.height/2 ) );
      float angle = atan2 (pose.dir.y, pose.dir.x) + PI;
      anchor.rotate(angle);
      anchor.addPt (pose.p);

      anchor.subPt (new pt(mouseX,mouseY));
      if ( anchor.norm() < 8 )
      {
        constrainPt.setFromMouse ();
        offsetPt.setFromMouse();
        offsetPt.subPt ( pose.p );
        offsetDir.setFromPt ( pose.dir);
        offsetDir.unit ();
        curControlPoint = i;
        return true;
      }

      anchor.setFromPt (pose.p);
      anchor.addPt (pose.dir);
      anchor.subPt ( new pt (mouseX, mouseY) );

      if (anchor.norm() < 3)
      {
        constrainPt.setFromMouse ();
        constrainPt.subPt(pose.dir);
        curAnchorPoint = i;
        return true; 
      }

    }

    return false;    
  }

  boolean handleReleaseEvent ()
  {
    if ( curControlPoint >= 0 )
    {
      pt mouse = new pt(mouseX,mouseY);
      if ( ! isLimited (mouse,upleftCurve,botrightCurve) )
        poses.remove(curControlPoint); 

      if ( poses.size() == 0 )
      {
        return true;
      }
    }

    curControlPoint = -1;
    curAnchorPoint = -1;
    return false; 
  }

  void handleDragEvent()
  {

    if ( curAnchorPoint >= 0 )
    {
      pt p = ((CarPose) poses.get(curAnchorPoint) ).dir;
      p.setFromMouse ();
      p.subPt (constrainPt);
      updateBezier (false);
      return;
    }

    if ( curControlPoint >= 0)
    {
      pt p = new pt(mouseX,mouseY);
      p.subPt (constrainPt);      
      pt dir = ((CarPose) poses.get(curControlPoint) ).dir;
      float n= dir.norm();
      dir.setFromPt (dir.add(p));
      dir.mul ( 1.0 / dir.norm() * n);
      pt newdir = dir.makeCopy();
      constrainPt.setFromMouse();
      pt pose = ((CarPose) poses.get(curControlPoint) ).p;
      float angle = atan2 ( offsetDir.y, offsetDir.x );//acos (dir.dot(offsetDir) / dir.norm());
      newdir.rotate(-angle);
      angle = atan2( newdir.y, newdir.x );
      pt off = offsetPt.makeCopy ();
      off.rotate(angle);

      pose.setFromMouse();
      pose.subPt(off);
      updateBezier (false);
    }
  }

  void addNewPos ()
  {

    int index = poses.size()-1;

    if (index >= 0 )
    {
      CarPose pos = (CarPose) poses.get(index);
      CarPose tmp = new CarPose ( pos.p.add (  pos.dir.sca(2.5) ), pos.dir.makeCopy() );
      if (isLimited ( tmp.p, upleftCurve.add( new pt(carSprite.width/2, carSprite.width/2 )), botrightCurve.sub ( new pt( carSprite.width/2, carSprite.width/2)) )) {
        poses.add (tmp);    
        updateBezier (true);
        return;
      }
    }

    pt dir = new pt (0,-40);
    pt p = new pt(height*4/5/2,width*7/8/2);
    poses.add ( new CarPose (p,dir ));

    updateBezier (true);
  }

  void updatePosFromTime (float t)
  {  
    if (curveControlled) {
      if ( poses.size() == 1)
      {
       CarPose pose = (CarPose)poses.get(0);
       curpos = pose.p.makeCopy();
       curangle = atan2 ( pose.dir.y, pose.dir.x);
       lastTime = t;
       return;
      }
    
      pt val = new pt(0,0);
      pt tangent = new pt(0,0);
      dc.sampleTime(t, val,tangent);
      bc.travelDist = val.y;
      bc.sampleAt (bc.travelDist,curpos,tangent);    
      curangle = atan2(tangent.y, tangent.x );
    }
    else
    {
      pt tvelo = new pt(0,0);
      pt tpos = new pt(0,0);
      pt tangle = new pt(0,0);
      timeStep (t-lastTime, tpos, tvelo, tangle, true );
      curpos = tpos;
      curvelo = tvelo;
      curangle = tangle.x;
      curanglevelo = tangle.y;
    }

    lastTime = t;
  }

  void tmpUpdatePosFromTime (float t)
  {
    if (curveControlled) {
      if ( poses.size() == 1)
      {
       CarPose pose = (CarPose)poses.get(0);
       tmppos = pose.p.makeCopy();
       tmpangle = atan2 ( pose.dir.y, pose.dir.x);
       return;
      }
      pt val = new pt(0,0);
      pt tangent = new pt(0,0);
      dc.sampleTime(t, val,tangent);
      bc.travelDist = val.y;
      bc.sampleAt (bc.travelDist,tmppos,tangent);    
      tmpangle = atan2(tangent.y, tangent.x );
    }
    else
    {
      pt tvelo = new pt(0,0);
      pt tpos = new pt(0,0);
      pt tangle = new pt(0,0);
      timeStep (t-lastTime, tpos, tvelo, tangle,false );
      tmppos = tpos;
      tmpangle = tangle.x;
    }
  }

  void timeStep (float dt, pt pos, pt velo, pt angle, boolean friction )
  {
    final int iters = 10;
    if (friction) {
    curvelo.mul(pow(0.995,velocityModifier));
    curanglevelo *= pow(0.995, velocityModifier);
    }
    pos.setFromPt (curpos);
    velo.setFromPt (curvelo);
    angle.x = curangle;
    angle.y = curanglevelo;

    for ( int i =0; i < iters; ++i)
    {
      pos.addPt( velo.sca(1.0/(float)iters*dt) );
      angle.x += angle.y / (float) iters*dt;
    }
  }
}

ArrayList cars = new ArrayList();