/* Circle class */
// Used in the program as our wheels
class Circle {
  /* Other properties */
  PVector position;
  float radius;
  float radiusSquared;
  
  /* Car specific properties */
  boolean wheel = false;
  // how much force the wheel exerts on its attached PointMass
  int force; 
  
  /* Which PointMass is the circle attached to? */
  boolean attachedToPointMass = false;
  PointMass attachedPointMass;
 
  boolean touchingGround;
  /* Constructor */
  Circle (PVector pos, float r) {
    position = pos.get();
    radius = r;
    radiusSquared = r*r;
    // add the circle to the world object so constraint solving is performed
    world.addCircle(this);
  }
  
  /* Constraint solving algorithm */
  // Here we find out if the circle is overlapping the surfacem, and act accordingly.
  void solveConstraints () {
    // At the end of the constraint solve, 
    // if this boolean is set to true and the player is pressing an up/down arrow key, the wheel accelerates
    touchingGround = false;

    // set the wheel's position to it's attached PointMass's position
    position = attachedPointMass.position.get();
    
    // if the wheel is overlapping the top of the screen, we push it down
    if (position.y < radius)
      position.y = 2*(radius) - position.y;

    /* Surface to Wheel collision */
    // Loop through x-coordinates the wheel occupies
    for (int i = (int)-radius; i < (int)radius; i++) {
      // The x coordinate
      int xPos = (int)position.x+i;
      
      // if the surface point at this x coordinate doesn't exist, then we generate it
      if (!surface.containsKey(xPos))
        surfaceGenerate(xPos);
      
      // Set a PVector using the xPos and the surface's height at the position
      PVector groundPoint = new PVector(xPos, (Float)surface.get(xPos));
      
      // avoid(PVector point) is a function inside this class 
      // that solves any collision between the point and this circle
      // it returns a boolean, true for if there was a collision, false if otherwise
      if (this.avoid(groundPoint)) {
        touchingGround = true;
        
        // surface friction is applied here:
        attachedPointMass.lastPosition.x += 0.1 * (attachedPointMass.position.x - attachedPointMass.lastPosition.x);
        attachedPointMass.lastPosition.y += 0.3 * (attachedPointMass.position.y - attachedPointMass.lastPosition.y);
      }
    }

    /* Here the wheel accelerates */
    if ((wheel) && (touchingGround)) {
      if (keys[40]) { // down
        attachedPointMass.applyForce(new PVector(-force, 0));
      }
      else if (keys[38]) { // up
        attachedPointMass.applyForce(new PVector(force, 0));
      }
    }  
    
    // Move the attached PointMass to the circle's position
    attachedPointMass.position = position.get();
  }
  
  /* Avoid */
  // Pushes the circle away from a static point
  // if there's a collision, it returns true
  // otherwise it returns false
  boolean avoid (PVector what) {
    // first we see if the point is inside the circle.
    PVector delta = PVector.sub(what, position);  
    if (radiusSquared > (sq(delta.x) + sq(delta.y))) {
      // find the distance
      float d = sqrt(delta.x * delta.x + delta.y * delta.y);

      // Move the circle based on how much the point overlaps it
      float difference = (radius - d) / d;
      position.sub(PVector.mult(delta, difference));
      return true;
    }
    return false;
  }
  
  /* The circle's draw function */
  void draw () {
    noFill();
    stroke(255);
    ellipse(position.x, position.y, radius*2, radius*2);
  }
  
  /* Set the attached PointMass */
  void attachToPointMass (PointMass p) {
    attachedPointMass = p;
  }
}

// The Link class is used for handling distance constraints between particles.
class Link {
  float restingDistance;
  float stiffness;
  
  PointMass p1;
  PointMass p2;
  
  // the scalars are how much "tug" the particles have on each other
  // this takes into account masses and stiffness, and is calculated in the Link constructor
  float scalarP1;
  float scalarP2;
  
  // set drawThis to false to make the link invisible
  boolean drawThis;
  color col = color(0);
  
  // Tearing isn't necessary, so I set it to a ridiculously high number.
  int tearThreshold = 100000;
  
  /* Constructor */
  Link (PointMass which1, PointMass which2, float restingDist, float stiff, boolean drawMe) {
    // When an object is ='d to another object, it's actually a reference, rather than a copy.
    // so stuff done to p1 and p2 here is actually done to the PointMasses in the World ArrayList
    p1 = which1;
    p2 = which2;
    
    restingDistance = restingDist;
    stiffness = stiff;
    
    // Masses are accounted for. If you remember the cloth simulator,
    // http://www.openprocessing.org/visuals/?visualID=20140
    // we added this ability in anticipation of future applets that might use mass
    float im1 = 1 / p1.mass; 
    float im2 = 1 / p2.mass;
    scalarP1 = (im1 / (im1 + im2)) * stiffness;
    scalarP2 = (im2 / (im1 + im2)) * stiffness;
    
    drawThis = drawMe;
  }
  
  /* Constraint solving */
  void solveConstraints () {
    // calculate the distance between the two particles
    PVector delta = PVector.sub(p1.position, p2.position);  
    float d = sqrt(delta.x * delta.x + delta.y * delta.y);
    float difference = (restingDistance - d) / d;
    
    // Tearing
    // In previous codes (Curtain, Ragdoll Aquarium), the link is removed
    // Here, (although not used), the link is split in half
    if (d > tearThreshold)  {
      p1.removeLink(this);
      if (drawThis) {
        this.snap(PVector.div(PVector.add(p2.position, p1.position),2));
      }
    }

    // P1.position += delta * ((1 / p1.mass) / ((1 / p1.mass) + (1 / p2.mass))) * stiffness * difference
    // P2.position -= delta * ((1 / p2.mass) / ((1 / p1.mass) + (1 / p2.mass))) * stiffness * difference
    p1.position.add(PVector.mult(delta, scalarP1 * difference));
    p2.position.sub(PVector.mult(delta, scalarP2 * difference));
  }
  
  /* Snap */
  // This function snaps the link at a certain position
  // Snap isn't used in this simulator, but it can be used in the future
  void snap (PVector pos) {
    /* If there's too many PointMasses, the link is just removed and the code is stopped */
    if (world.pointMasses.size() > 2500) {
      p1.removeLink(this);
      return;
    }

    // Remove the link so we can create 2 smaller links
    p1.removeLink(this);

    // Find the distances between each PointMass and the pos and use it as a ratio for the restingDistance
    float dist1 = dist(pos.x, pos.y, p1.position.x, p1.position.y);
    float dist2 = dist(pos.x, pos.y, p2.position.x, p2.position.y);
    
    // if the link's already really small, we just keep it removed
    if (dist1 + dist2 > 4) {
      // Calculate the distances for each side
      float p1OwnerLength = (dist1/(dist1+dist2)) * restingDistance;
      float p2OwnerLength = (dist2/(dist1+dist2)) * restingDistance;
      
      // Create link on one side
      PointMass p1Owner = new PointMass(pos);
      p1Owner.lastPosition = pos.get();
      p1Owner.mass = p2.mass/2;
      p2.mass = p2.mass/2;
      p1Owner.attachTo(p1, p1OwnerLength, stiffness, drawThis, color(0,0,0));
      
      // Create link on the other side
      PointMass p2Owner = new PointMass(pos);
      p2Owner.lastPosition = pos;
      p2Owner.mass = p1.mass/2;
      p1.mass = p1.mass/2;
      p2Owner.attachTo(p2, p2OwnerLength, stiffness, drawThis, color(0,0,0));
      
      // Set the links' tearThresholds
      Link newLink = (Link) p1Owner.links.get(p1Owner.links.size()-1);
      newLink.tearThreshold = tearThreshold;
      Link newLink2 = (Link) p2Owner.links.get(p2Owner.links.size()-1);
      newLink2.tearThreshold = tearThreshold;
    }
  }
  /* Draw */
  void draw () {
    // Make sure the line will be 1 pixel wide
    strokeWeight(1);
    if (drawThis) { // some links are invisible
      stroke(col);
      line(p1.position.x, p1.position.y, p2.position.x, p2.position.y);
    }
  }
}

// PointMass
// This is pretty much the Particle class used in Curtain
// http://www.openprocessing.org/visuals/?visualID=20140
class PointMass {
  // for calculating position change (velocity)
  PVector lastPosition;
  PVector position;
  PVector acceleration; 

  float mass = 1;

  // An ArrayList for links, so we can have as many links as we want to this PointMass
  ArrayList links = new ArrayList();

  // For pinning the PointMass
  // This isn't used in this code
  boolean pinned = false;
  PVector pinLocation = new PVector(0, 0);

  
  boolean attachedToMouse = false;

  boolean touchingGround = false;
  /* PointMass constructor */
  PointMass (PVector pos) {
    position = pos.get();
    lastPosition = pos.get();
    acceleration = new PVector(0, 0);
    
    // add the PointMass to the world object
    world.addPointMass(this);
  }
  
  /* Physics */
  // The update function is used to update the physics of the particle.
  // motion is applied, and links are drawn here
  void updatePhysics (float timeStep) { 
    // gravity:
    // f(gravity) = m * g
    PVector fg = new PVector(0, mass * world.gravity, 0);
    this.applyForce(fg);
  
    /*
       We use Verlet Integration to simulate the physics
       In Verlet Integration, the rule is simple: any change in position will result in a change of velocity
       Therefore, things in motion will stay in motion. If you want to push a PointMass towards a direction,
       just move its position and it'll continue going that way.   
    */
    // velocity = position - lastPosition
    PVector velocity = PVector.sub(position, lastPosition);
    // Slight friction is applied
    velocity.mult(0.999); 
    // newPosition = position + velocity + 0.5 * acceleration * deltaTime * deltaTime
    PVector nextPos = PVector.add(PVector.add(position, velocity), PVector.mult(PVector.mult(acceleration, 0.5), timeStep * timeStep));
    // reset the variables
    lastPosition.set(position);
    position.set(nextPos);
    acceleration.set(0, 0, 0);
  
    // make sure the particle stays in its place if it's pinned
    // (This isn't used for this simulation, but it's there anyways.)
    if (pinned)
      position.set(pinLocation);
  }
  
  /* Interaction updating */
  void updateInteractions () {
    // this is where our interaction comes in.
    if (mousePressed) {
      if (mouseAttachedToSomething == false) {
        float distanceSquared = sq(tmouseX - position.x) + sq(mouseY - position.y);
        if (mouseButton == LEFT) {
          // if (dist * dist < radius * radius), 
          // remember mouseInfluenceSize was squared in setup()
          if (distanceSquared < mouseInfluenceSize) { 
            // Attach to mouse
            attachedToMouse = true;
            mouseAttachedToSomething = true;
            attachedPM = this;
          }
        }
        else { // if the right mouse button is clicking, we tear this by removing links
          if (distanceSquared < mouseTearSize) 
            links.clear();
        }
      }
    }
    // if the player isn't clicking, we make sure the PointMass isn't still attached to the cursor
    else if (attachedToMouse) {
      mouseAttachedToSomething = false;
      attachedToMouse = false;
    }
  }
  
  /* Draw */
  void draw () {
    // draw the links and points
    stroke(0);
    if (links.size() > 0) {
      for (int i = 0; i < links.size(); i++) {
        Link currentLink = (Link) links.get(i);
        // Use the link's draw function
        currentLink.draw();
      }
    }
    else
      point(position.x, position.y);
  }
  
  /* Constraint Solving */
  // here we tell each Link to solve constraints
  void solveConstraints () {
    touchingGround = false;
    
    for (int i = 0; i < links.size(); i++) {
      Link currentLink = (Link) links.get(i);
      currentLink.solveConstraints();
    }

    // Make sure the particle isn't intersecting the ground
    // First we generate the surface if it doesn't exist
    if (surface.containsKey((int)position.x) == false)
      surfaceGenerate((int)position.x);
    // Then we move the PointMass up if it's below the surface at the PointMass's x position
    float ground = (Float)surface.get((int)position.x);
    if (position.y > ground-1) {
      position.y = 2 * (ground - 1) - position.y;
      lastPosition.x = (position.x + lastPosition.x)/2;
      touchingGround = true;
    }
      
    // Make sure the PointMass isn't above the screen
    if (position.y < 1)
      position.y = 2 - position.y;
    
    // Move the PointMass to the cursor if it's attached
    if (attachedToMouse)
      position.set(tmouseX, mouseY, 0);
  }

  /* Attaching Links */
  // attachTo can be used to create links between this particle and other particles
  Link attachTo (PointMass P, float restingDist, float stiff, boolean drawThis) {
    Link lnk = new Link(this, P, restingDist, stiff, drawThis);
    
    links.add(lnk);
    return lnk;
  }
  // same as the first attachTo, but with colors!
  Link attachTo (PointMass P, float restingDist, float stiff, boolean drawThis, color col) {
    Link lnk = new Link(this, P, restingDist, stiff, drawThis);
    lnk.col = col;

    links.add(lnk);
    return lnk;
  }
  
  /* Removing Links */
  void removeLink (Link lnk) {
    links.remove(lnk);
  }  
  void removeLink (PointMass P) {
    for (int i = 0; i < links.size(); i++) {
      Link lnk = (Link) links.get(i);
      if ((lnk.p1 == P) || (lnk.p2 == P))
        links.remove(i);
    }
  }

  void applyForce (PVector f) {
    // acceleration = (1/mass) * force
    // or
    // acceleration = force / mass
    acceleration.add(PVector.div(f, mass));
  }

  void pinTo (PVector location) {
    pinned = true;
    pinLocation.set(location);
  }
}

// Wheel that's touching the ground
Circle wheelTouchingGround;
// current score
float score;
// top score
float topScore;
// where the person has started balancing on one wheel
float startX = 0;
/* Reset Score */
// The score is reset and the "wheelTouchingGround" is removed
// will be called whenever the other wheel or a corner of the car touches the ground
void resetScore () {
  if (score > topScore)
    topScore = score;
  score = 0;
  wheelTouchingGround = null;
}
/* Set score */
void setScore (float value) {
  // If the new score is lower and the old score is higher than the top score
  // we update the top score
  if ((value < score) && (score > topScore))
    topScore = score;
  score = value;
}
/* Update Score */
// Here we check for faults, and reset/set the score accordingly.
void updateScore () {
  // if both wheels are touching the ground
  if ((car.frontWheel.touchingGround) && (car.backWheel.touchingGround)) {
    resetScore();
    return;  
  }
  // If any corner is touching the ground
  if ((car.front.touchingGround) || (car.back.touchingGround))
    resetScore();
  // If the frontWheel is touching the ground, but not the backwheel
  if ((car.frontWheel.touchingGround) && (!car.backWheel.touchingGround)) {
    // If the frontWheel wasn't the wheel that initially was touching the ground, we reset everything
    if (car.frontWheel != wheelTouchingGround) {
      resetScore();
      wheelTouchingGround = car.frontWheel;
      startX = car.frontWheel.position.x;
    }
    // The score is the distance between startX and the wheel position
    setScore(abs(startX - wheelTouchingGround.position.x)/100);
  }
  // same as above but for the backWheel.
  if ((car.backWheel.touchingGround) && (!car.frontWheel.touchingGround)) {
    if (car.backWheel != wheelTouchingGround) {
      resetScore();
      wheelTouchingGround = car.backWheel;
      startX = car.backWheel.position.x;
    }
    setScore(abs(startX - wheelTouchingGround.position.x)/100);
  }
  // If the user is mouseclicking and trying to pick up the car, the score is reset.
  if (mousePressed)
    resetScore();
}
/* Update Score Board */
// Used to rewrite the Score: and Top Score: text on the top left of the screen
void updateScoreBoard () {
  // Draw it on the scoreBoard PGraphics
  scoreBoard.beginDraw();
  scoreBoard.background(0,0,0,0);
  scoreBoard.fill(255);
  scoreBoard.text("Score: " + (float)round(score*10) / 10, 15, 30);
  scoreBoard.text("Top Score: " + (float)round(topScore*10) / 10, 15, 60);
  scoreBoard.endDraw();
  // Draw the scoreBoard PGraphics onto the main screen.
  image(scoreBoard, width/2 - lastX,0);
}

/* World */
// All physics and objects, as well as the time step stuff, are handled here.
class World {
  /* All of our objects */
  ArrayList pointMasses = new ArrayList(); 
  ArrayList circles = new ArrayList();
  
  float gravity = 392 * 2;
  
  // These variables are used to keep track of how much time is elapsed between each frame
  // they're used in the physics to maintain a certain level of accuracy and consistency
  // this program should run the at the same rate whether it's running at 30 FPS or 300,000 FPS
  long previousTime;
  long currentTime;
  // Delta means change. It's actually a triangular symbol, to label variables in equations
  // some programmers like to call it elapsedTime, or changeInTime. It's all a matter of preference
  // To keep the simulation accurate, we use a fixed time step
  int fixedDeltaTime = 10;
  float fixedDeltaTimeSeconds;
  // the leftOverDeltaTime carries over change in time that isn't accounted for over to the next frame
  int leftOverDeltaTime;

  // How many times constraints are solved each frame
  int constraintAccuracy;
  
  /* Constructor */
  World (int deltaTimeLength, int constraintAcc) {
    fixedDeltaTime = deltaTimeLength;
    fixedDeltaTimeSeconds = (float)fixedDeltaTime / 1000;
    previousTime = millis();
    currentTime = previousTime;  
    constraintAccuracy = constraintAcc;
  }
  void update () {
    /* Time related stuff */
    currentTime = millis();
    // deltaTimeMS: change in time in milliseconds since last frame
    long deltaTimeMS = currentTime - previousTime;
    // Reset previousTime.
    previousTime = currentTime;
    // timeStepAmt will be how many of our fixedDeltaTime's can fit in the physics for this frame.
    int timeStepAmt = (int)((float)(deltaTimeMS + leftOverDeltaTime) / (float)fixedDeltaTime);
    // reset leftOverDeltaTime.
    leftOverDeltaTime = (int)deltaTimeMS - (timeStepAmt * fixedDeltaTime);
    // If the program is running too slow, we limit the accumulation to prevent freezing
    if (leftOverDeltaTime > 500)
      leftOverDeltaTime = 500;
    float fixedDeltaTimeSeconds = (float)fixedDeltaTime / 1000;
    
     /* Physics */
    for (int iteration = 1; iteration <= timeStepAmt; iteration++) {
      // update each PointMass's position
      for (int i = 0; i < pointMasses.size(); i++) {
        PointMass p = (PointMass) pointMasses.get(i);
        p.updatePhysics(fixedDeltaTimeSeconds);
      }
      
      // constraint solve multiple times for higher accuracy
      for (int x = 0; x < constraintAccuracy; x++) { 
        for (int i = 0; i < pointMasses.size(); i++) {
          PointMass p = (PointMass) pointMasses.get(i);
          p.solveConstraints();
        }
        for (int i = 0; i < circles.size(); i++) {
          Circle c = (Circle) circles.get(i);
          c.solveConstraints();  
        }
      }
      
      /* Update the car */
      car.update();
      /* Update the score */
      updateScore();
    }
    // we use a separate loop for drawing so points and their links don't get drawn more than once
    // (rendering can be a major resource hog if not done efficiently)
    // also, interactions (mouse dragging) is applied
    for (int i = 0; i < pointMasses.size(); i++) {
      PointMass p = (PointMass) pointMasses.get(i);
      p.updateInteractions();
      p.draw();
    }
    for (int i = 0; i < circles.size(); i++) {
      Circle c = (Circle) circles.get(i);
      c.draw();  
    }
  }
  
  // Functions for adding PointMasses and Circles.
  void addCircle (Circle c) {
    circles.add(c);  
  }
  void removeCircle (Circle c) {
    circles.remove(c);  
  }
  void addPointMass (PointMass p) {
    pointMasses.add(p);
  }
  void removePointMass (PointMass p) {
    pointMasses.remove(p);
  }
}

/*
 Car
 Made by Jared "BlueThen" Counts on April 30, 2011
 Updated May 4, 2011.
 www.bluethen.com
 www.twitter.com/BlueThen
 www.openprocessing.org/portal/?userID=3044
 www.hawkee.com/profile/37047/
 bluethen ( @ ) gmail.com
 
 Up/Down to accelerate or deaccelerate
 Left/Right to tilt
 Click and drag to pick up the car
 
 To get a higher score, drive on one wheel for as far as possible
*/

// World world: object where physics is handled
World world;

// scoreBoard: the onscreen text displaying the score
// we use a separate PGraphics for this because P2D (the main renderer) 
// draws crappy text
PGraphics scoreBoard;
PFont font;

/* Surface and surface properties */
// "Map" object is actually Java's interface for storing objects and retrieving them using keys
// in our case, the key will be the x coordinate, and the object returned is the surface's height
Map surface;
// higher amplitude for higher hills
float surfaceAmplitude = 300;
// higher frequency for rougher/jagged hills
float surfaceFrequency = 0.0025;

// The player's car
Car car;

/* Mouse stuff */
float mouseInfluenceSize = sq(8); 
// Tearing isn't really necessary, but we keep it anyways
// this is leftover from the Curtain applet @ http://bluethen.com/wordpress/index.php/processing-apps/curtain/
float mouseTearSize = sq(8);
boolean mouseAttachedToSomething = false;
PointMass attachedPM;

// translated mouse X position
int tmouseX;
//  firstX and lastX are the screen's first and last X coordinates, for translating
float firstX = width/2;
int lastX;

long seed = (long)random(1000000000);
/* Setup */
// Here everything is initialized (or set up) for the simulation
void setup () {
  /* rendering stuff */
  size(640, 480, P2D); 
  colorMode(HSB, 255);
  smooth();
  
  /* Initialize the scoreboard */
  // populationSign is rendered with JAVA2D because text renedered in P2D is ugly
  scoreBoard = createGraphics(300,200, JAVA2D);
  font = loadFont("data\\ShowcardGothic-Reg-24.vlw");
  scoreBoard.beginDraw();
  scoreBoard.textFont(font, 24);
  scoreBoard.endDraw();
  
  /* The object that handles all of the physics */
  // Parameters: TimeStep size (smaller is more accurate, but slower), constraint solve iterations (bigger is more accurate, but slower)
  // Feel free to experiment with different parameters
  world = new World(15, 3);

  /* Generate the surface for our vehicle */
  // Make sure the noise seed is consistent.
  surface = new HashMap();
  // if you ever want to play the same track twice, use this with seed equalling any long integer
  noiseSeed(seed);
  // only generate a surface for 0 to width
  for (int i = 0; i < width; i++) {
    // we use a "surfaceGenerate" function found at the bottom of car.pde (this file).
    surfaceGenerate(i); // surface.put(i, height - 300*noise(i*0.0025));
  }
  
  /* The player's car */
  // Parameters: PVector spawnPosition, int width (in pixels), int height (in pixels), int wheelRadius, int speed (as a force)
  car = new Car(new PVector(width/2, 100), 60, 30, 20, 10000);
  
  /* Race! This generates 20 vehicles, all simultaneously controlled by your Up/Down arrow keys */
  // Uncomment to race!
//  for (int i = 0; i < 20; i++) {
//    Car car = new Car(new PVector(random(width), random(height)), (int)random(10,100), (int)random(5,50), (int)random(5,30), (int)random(5000,20000));  
//  }
}
/* Draw */
// This function is iterated over and over by processing. It's our frame for the game
void draw () {
  // Erase everything with black
  background(0);

  // Make sure the noise seed is consistent.
  noiseSeed(seed);
  
  /* Translating */
  // Move the screen towards the car
  firstX += 0.2 * (((width/2 - (car.back.position.x + car.front.position.x)/2) - width/2) - firstX);
  lastX = (int)firstX + width;
  translate(firstX + width/2, 0);
  // Set a variable for the mouseX with translation applied, so the user can pick up the car
  tmouseX = mouseX + width/2 - lastX;

  /* Surface generating and displaying */
  for (int i = (int)firstX; i < lastX; i++) {
    int xPos = width/2-i;
    
    // Check if the surface at this point exists
    // if it doesn't, we generate one for it
    if (surface.containsKey(xPos) == false)
      surfaceGenerate(xPos);
    if (surface.containsKey(xPos-1) == false)
      surfaceGenerate(xPos-1);
      
    // Make it colorful
    stroke(abs(xPos) % 255, 255, 255);
    line(xPos, (Float)surface.get(xPos), xPos, height);
  }
  /* Update the physics */
  world.update();
  
  /* Update the on-screen score */
  updateScoreBoard();
  
  /* For the developer to keep track of the frameRate and surface size 
     The surface grows indefinitely, but it does not slow down the program, even after 1 million points
     so nothing is done about it for now. 
  */
  if (frameCount % 60 == 0)
    println("Frame Rate: " + frameRate + ", Surface size: " + surface.size());
}
/* Generate the surface at a single point */
void surfaceGenerate (int xPos) {
  // translation + amplitude * noise(iteration * frequency)
  surface.put(xPos, height - surfaceAmplitude * noise(xPos * surfaceFrequency));
}

/* Car class */
// At the moment, it's only used to create the car
// In the future, we could make some changes in the update function
// for turrets, drivers, etc
class Car {
  /* Parts */
  PointMass front;
  PointMass back;
  PointMass frontBottom;
  PointMass backBottom;
  
  PointMass frontSpring;
  PointMass backSpring;
  
  Circle frontWheel;
  Circle backWheel;
  
  /* Properties */
  int wide;
  int tall;
  int speed;
  
  // Direction the car is moving
  boolean movingForward = true;
  /* Constructor */
  Car (PVector pos, int wid, int tal, int wheelRadius, int speed) {
    wide = wid;
    tall = tal;
    
    // a and b are the spring lengths
    float a = tall;
    float b = 1.7 * tall;
    
    // Create front and back PointMasses
    front = new PointMass(new PVector(pos.x-(wide/2),pos.y));  
    back = new PointMass(new PVector(pos.x+(wide/2),pos.y));
    front.mass = 1.5;
    back.mass = 1.5;
    
    /* Create front and back bottom PointMasses */
    frontBottom = new PointMass(new PVector(pos.x-(wide/2),pos.y+tall));
    backBottom = new PointMass(new PVector(pos.x+(wide/2),pos.y+tall));
    frontBottom.mass = 3;
    backBottom.mass = 3;
    
    /* Create Spring PointMasses (the wheels attach to these */
    frontSpring = new PointMass(new PVector(pos.x-(wide/2),pos.y+40));
    backSpring = new PointMass(new PVector(pos.x+(wide/2),pos.y+40));
    frontSpring.mass = 2;
    backSpring.mass = 2;
        
    /* Attach the springs to each other */     
    front.attachTo(back, wide, 1, true, color(255));
    
    frontBottom.attachTo(front, tall, 1, true, color(255));
    backBottom.attachTo(back, tall, 1, true, color(255));
    
    frontBottom.attachTo(backBottom, wide, 1, true, color(255));
    backBottom.attachTo(frontBottom, wide, 1, true, color(255));
    
    frontBottom.attachTo(back, sqrt(sq(tall) + sq(wide)), 1, true, color(255));
    backBottom.attachTo(front, sqrt(sq(tall) + sq(wide)), 1, true, color(255));
    
    frontSpring.attachTo(frontBottom, a, 0.1, true, color(255));
    frontSpring.attachTo(front, b, 0.1, true, color(255));
    backSpring.attachTo(backBottom, a, 0.1, true, color(255));
    backSpring.attachTo(back, b, 0.1, true, color(255));
    

    // we use some fancy trig to find out the distance between the Springs and the top corners of the car
    // it might be a bit more convenient to just plot out the points on an XY grid and use dist() for the restingDistances
    // oh well!
    double theta = acos( (sq(tall) - sq(a) - sq(b)) / (-2 * a * b) );
    double theta2 = asin((a * sin(new Float(theta))) / tall);
    double theta5 = 90 - theta2 - theta;
    double d = a * cos(new Float(theta5));
    double e = a * sin(new Float(theta5));
    backSpring.attachTo(front, sqrt(sq((float)d + wide) + sq((float)e + tall)), 0.4, true, color(255));
    frontSpring.attachTo(back, sqrt(sq((float)d + wide) + sq((float)e + tall)), 0.4, true, color(255));

    /* Create the wheels */
    frontWheel = new Circle(frontSpring.position, wheelRadius);
    backWheel = new Circle(backSpring.position, wheelRadius);
    frontWheel.wheel = true;
    backWheel.wheel = true;
    frontWheel.force = speed;
    backWheel.force = speed;
    
    frontWheel.attachToPointMass(frontSpring);
    backWheel.attachToPointMass(backSpring);
  }
  /* Update */
  // Used mostly for calculating tilt based on what the user's pressing
  void update() {
    /* Center Of Mass */
    /* 
      The center of mass is calculated by taking each pointmass position,
      multiplying their mass, adding them together, then dividing by total mass
      (P1.pos * P1.mass + P2.pos * P2.mass + ...) / (P1.mass + P2.mass + ...)   
    */
    PVector cent = PVector.mult(frontBottom.position.get(), frontBottom.mass);
    cent.add(PVector.mult(backBottom.position, backBottom.mass));
    cent.add(PVector.mult(front.position, front.mass));
    cent.add(PVector.mult(back.position, back.mass));
    cent.add(PVector.mult(frontSpring.position, frontSpring.mass));
    cent.add(PVector.mult(backSpring.position, backSpring.mass));
    cent.div(frontBottom.mass + backBottom.mass + front.mass + back.mass + frontSpring.mass + backSpring.mass);
    
    if (keys[37]) { // Left
      // The car is tilted based on the direction it's moving
      // if we just moved one pointmass, or both, the car will turn awkwardly and float on some occasions
      if (movingForward) {
        // Calculate the angle perpendicular from the centerOfMass to back.position
        float angle = atan2(back.position.y - cent.y, back.position.x - cent.x) - PI/2;
        // Apply force
        back.applyForce(new PVector(7500 * cos(angle),
                                    7500 * sin(angle)));
      }
      else {
        float angle = atan2(front.position.y - cent.y, front.position.x - cent.x) - PI/2;
        front.applyForce(new PVector(7500 * cos(angle),
                                     7500 * sin(angle)));
      }                      
    }
    else if (keys[39]) { // Right
      if (movingForward) {
        float angle = atan2(back.position.y - cent.y, back.position.x - cent.x) + PI/2;
        back.applyForce(new PVector(7500 * cos(angle),
                                    7500 * sin(angle)));
      }
      else {
         float angle = atan2(front.position.y - cent.y, front.position.x - cent.x) + PI/2;
         front.applyForce(new PVector(7500 * cos(angle),
                                      7500 * sin(angle)));
      }                         
    }
  }
}

/**
multiplekeys taken from http://wiki.processing.org/index.php?title=Keep_track_of_multiple_key_presses
@author Yonas Sandbæk http://seltar.wliia.org
*/
 
// usage: 
// if(checkKey("ctrl") && checkKey("s")) println("CTRL+S");  
// Note: DOES NOT WORK WITH MACS
boolean[] keys = new boolean[526];
boolean checkKey(String k) {
  for(int i = 0; i < keys.length; i++)
    if(KeyEvent.getKeyText(i).toLowerCase().equals(k.toLowerCase())) return keys[i];  
  return false;
}
 
void keyPressed() { 
  keys[keyCode] = true;
  println(KeyEvent.getKeyText(keyCode) + " " + keyCode);
  if (keys[40]) { // down
    car.movingForward = false;
  }
  else if (keys[38]) { // up
    car.movingForward = true;
  }
}
void keyReleased() { 
  keys[keyCode] = false;
}