// S H A D O W  E L E M E N T S

// GLOBAL CONSTANTS

// Size of window
final int X_SIZE = 600;
final int Y_SIZE = 600; 
// Background image
final String imageName = "shadow2map.jpg";
BImage bgImage;

// Number of vehicles on the image
final int NUM_VEHICLES = 50;
final int VEHICLE_WIDTH = 5;
final int VEHICLE_LENGTH = 8;
final color VEHICLE_COLOR = #FF0000;
// Increasing this increases the speed they travel
// also makes it harder for them to turn sharply
final float SPEED_MULT = 8;

// Vehicle state information stored in this array
// of objects of class Vehicle
// We have not yet discussed objects and classes. (But we will.)
Vehicle[] vehicles = new Vehicle[NUM_VEHICLES];

// Leave trails as they move?
final boolean LEAVE_TRAILS = true;
// Amounts to change the underlying image color when leaving a trail
final color   TRAIL_COLOR = #44CCE0;  // The final color of the trail
final float   CHANGE_PER_TRAIL = 0.1; // the amount toward that color for each pass

// Framerate in frames-per-second
final int FRAME_RATE = 60;

//orient variables
double hx = 0.0;
double hy = 0.0;
double px = 0.0;
double py = 0.0;
double ex = 0.0;
double ey = 0.0;
double tx = 0.0;
double ty = 0.0;
double rx = 0.0;
double ry = 0.0;
double sf = 1.0;
double sp = 1.0;

// --------------- S E T U P -------------------------------
// Initialize the starting conditions
// Randomly assign values to all cells
void setup()
{
  size(X_SIZE, Y_SIZE);
  bgImage = loadImage(imageName);
  makeVehicles();
  framerate(FRAME_RATE);
}

// --------------- L O O P -------------------------------
void loop()
{
  orient();
  background(255);
  drawVehicles();
  drawImage();
  
  calculateNextVehicleStates();
  updateVehicleStates();
  
}

// -------------- P O I N T ------------------------------
public class Point
{
  double x;
  double y;
  double z;

  public Point()
  {
    this(0.0, 0.0, 0.0);
  }

  public Point(double x, double y, double z)
  {
    this.x = x;
    this.y = y;
    this.z = z;
  }

  public Point(Point v)
  {
    this.x = v.x;
    this.y = v.y;
    this.z = v.z;
  }

  public double Length()
  {
    return Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z);
  }

  public Point normalize()
  {
    double l = this.Length();
    if (l != 0)
    {
      this.x /= l;
      this.y /= l;
      this.z /= l;
    }

    return this;
  }

  public Point add(Point v)
  {
    this.x += v.x;
    this.y += v.y;
    this.z += v.z;

    return this;
  }

  public Point add(double x, double y, double z)
  {
    return this.add(new Point(x, y, z));
  }

  public Point sub(Point v)
  {
    this.x -= v.x;
    this.y -= v.y;
    this.z -= v.z;

    return this;
  }

  public Point sub(double x, double y, double z)
  {
    return this.sub(new Point(x, y, z));
  }

  public Point mult(double k)
  {
    this.x *= k;
    this.y *= k;
    this.z *= k;

    return this;
  }

  public Point div(double k)
  {
    this.x /= k;
    this.y /= k;
    this.z /= k;

    return this;
  }

  public Point cross(Point v)
  {
    double a = this.y*v.z - this.z*v.y;
    double b = this.z*v.x - this.x*v.z;
    double c = this.x*v.y - this.y*v.x;

    return new Point(a, b, c);
  }

  public String toString()
  {
    return "(" + x + ", " + y + ", " + z + ")";
  }
}


// --------------------------- C L A S S  P L I N E ----------------------------
public class pLine
{
  pLine(Point po, Point pi)
  {
    line(int(po.x), int(po.y), int(po.z), int(pi.x), int(pi.y), int(pi.z));
  }
}


// ----------------------- C L A S S   V E H I C L E --------------
class Vehicle {

  float x, y, z, direction;    // Location and direction (direction is in radians)
  float nextX, nextY, nextDirection;  // Location and direction to update to
  float lSensorX, lSensorY, rSensorX, rSensorY;  // Left and right sensor locations
  float lSensorVal, rSensorVal;  // Left and right sensor values
  float width, length;           // Width and length of the vehicle (set by default to VEHICLE_WIDTH and VEHICLE_HEIGHT)
  int   type;                    // What type of vehicle is this?
                                 // Right now we are only using one type (0), but you might want to define more.
  color trailColor = TRAIL_COLOR;  // The color of the trail. Likely different for every vehicle type.
  color vehicleColor = VEHICLE_COLOR; // The color of the vehicle.
  boolean onEdge;                  // Have we hit an edge?
  Point[] trail = new Point[10000]; //holds trail for vehicle
  int trailcount;
  
// ------------------ V E H I C L E ---------------------------------
// Constructor function. This is called each time a new vehicle object is
// created. Randomly assigns state values. 
  Vehicle(int aType) {
    type = aType;
    x = random(X_SIZE);
    y = random(Y_SIZE);
    trailcount = 0;
    onEdge = false;
    
    direction = random(0, 360.0);
    width = VEHICLE_WIDTH;
    length = VEHICLE_LENGTH;
  }
  
// ------------------ V E H I C L E  .  U P D A T E   S T A T E ---------------------------------
// Update the vehicle's internal state with the new values
  void updateState() {

    x = nextX;
    y = nextY;
    
    //brightness controls the z-dimension
    z = 300 - (300 * (brightness(bgImage.get((int)lSensorX, (int)lSensorY)) / 255.0));
    if (z > 299) {
      z = 0;
    }
    
    trail[trailcount] = new Point(x,y,z);
    trailcount++;
    
    direction = nextDirection;
    updateSensorPositions();  // Update where the sensors are
    
    // Leave the trail sticking out of the middle of the back of the vehicle
    if (LEAVE_TRAILS) {
      float dx2 = -(width/2) * sin(HALF_PI - direction);
      float dy2 = (width/2) * cos(HALF_PI - direction);
      int trailX = int(x - dx2);
      int trailY = int(y + dy2);
      int index = trailY*X_SIZE + trailX;
      if (index < 0 || index >= X_SIZE * Y_SIZE) {
        return;
      }
      color c = bgImage.pixels[index];
      int dRed = int( (red(trailColor) - red(c)) * CHANGE_PER_TRAIL);
      int dGreen = int( (green(trailColor) - green(c)) * CHANGE_PER_TRAIL);
      int dBlue = int( (blue(trailColor) - blue(c)) * CHANGE_PER_TRAIL);
      
      bgImage.pixels[index] = color(red(c)+dRed,green(c)+dGreen,blue(c)+dBlue);
    }
  }
  
// ------------------ V E H I C L E  .  U P D A T E   S E N S O R   P O S I T I O N S ---------------------------------
// Update where the vehicle's sensors are
  void updateSensorPositions() {
    float dx = -length * sin(direction);
    float dy = length * cos(direction);
    
    float dx2 = -width * sin(HALF_PI - direction);
    float dy2 = width * cos(HALF_PI - direction);
        
    rSensorX = x + dx;
    rSensorY = y + dy;

    lSensorX = rSensorX - dx2;
    lSensorY = rSensorY + dy2;
  }
  
// ------------------ V E H I C L E  .  R E A D   S E N S O R S ---------------------------------
// Get values from the sensors (right now senses brightness only)
// You could change that to have them sense whatever you want.
  void readSensors() {    
    if (lSensorY < 0 || lSensorY > Y_SIZE || lSensorX < 0 || lSensorX > X_SIZE) {
      lSensorVal = 0;
      onEdge = true;
    }
    else {
      lSensorVal = SPEED_MULT * brightness(bgImage.get((int)lSensorX, (int)lSensorY)) / 255.0;
    }
    
    if (rSensorY < 0 || rSensorY > Y_SIZE || rSensorX < 0 || rSensorX > X_SIZE) {
      rSensorVal = 0;
      onEdge = true;
    }
    else {
      rSensorVal = SPEED_MULT * brightness(bgImage.get((int)rSensorX, (int)rSensorY)) / 255.0;
    }
  }
  
// ------------------ V E H I C L E  .  C A L C U L A T E   N E X T   S T A T E ---------------------------------
// Determine what the nest state should be depending on the sensor values
  void calculateNextState() {
    readSensors();
    
    // If you've hit an edge, reposition randomly
    if (onEdge) {
      nextDirection = random(TWO_PI);
      onEdge = false;
      return;
    }
    
    if (x < 0) {
      nextX = 0;
    }
    
    if (y < 0) {
      nextY = 0;
    }

    // Here's where you'd cross sensor wires. vL and vR are velocity of left and right
    // wheels, and lSensorVal and rSensorVal are the left and right sensor values.
    // A lot of behavior can be changed by messing with these two lines.
    float vL = lSensorVal;
    float vR = rSensorVal;    
    
    float dRot = atan2((vR - vL), width);
    float avg = (vR + vL) / 2.0;
    float dy = cos(direction + (dRot / 2.0)) * avg;
    float dx = -sin(direction + (dRot / 2.0)) * avg;

    nextX = x + dx;
    nextY = y + dy;
    nextDirection = direction + dRot;
    if (nextDirection > TWO_PI) {
      nextDirection -= TWO_PI;
    }

  }
  
// ------------------ V E H I C L E  .  D R A W ---------------------------------
// Draw the vehicle
  void draw() {
    int ix = int(x);
    int iy = int(y);
    int iz = int(z);
    pLine p;
    
    noStroke();
    fill(vehicleColor);
    
    push();
    translate(ix - width, iy, iz);
    rotate(direction);
    box(5);
    pop();
    
    stroke(TRAIL_COLOR);
    if (trailcount > 1) {
      for (int i = 0; i < (trailcount - 1); i++) {
      p = new pLine(trail[i], trail[i+1]);
      }
    }
  }
}

// --------------- M A K E    V E H I C L E S -------------------------------
// Initialize all the vehicle data randomly
void makeVehicles() {
  for (int i = 0; i < NUM_VEHICLES; i++) {
    vehicles[i] = new Vehicle(0);
  }
}

// --------------- C A L C U L A T E   N E X T   V E H I C L E   S T A T E S --------------
// Loop through and calculate the next state for each vehicle
void calculateNextVehicleStates() {
  for (int i = 0; i < NUM_VEHICLES; i++) {
    vehicles[i].calculateNextState();
  }
}

// --------------- U P D A T E   V E H I C L E   S T A T E S --------------
// Loop through and give each vehicle its next state
void updateVehicleStates() {
  for (int i = 0; i < NUM_VEHICLES; i++) {
    vehicles[i].updateState();
  }
}

// --------------- D R A W   V E H I C L E S ------------------------------
// Draw each vehicle
void drawVehicles() {
  for (int i = 0; i < NUM_VEHICLES; i++) {
    vehicles[i].draw();
  }
}

// -------------- D R A W   I M A G E ------------------------------------
// Draw the background image (continuously effaced with trails)
void drawImage() {
  color c;
  noStroke();
  
  for (int i = 0; i < bgImage.width; i = i + 10){
    for (int ii = 0; ii < bgImage.height; ii = ii + 10){
      c = bgImage.get(i, ii);
      fill(c);
      
      push();
      translate(i, ii, 0);
      fill(c);
      box(8,8,0);
      pop();
    }
  }
}


// --------------- M O U S E P R E S S E D ----------------------
void mousePressed( ) {
  hx = mouseX;
  hy = mouseY;
  px = tx;
  py = ty;
  ex = rx;
  ey = ry;
  sp = sf;
}

// ---------------  M O U S E D R A G G E D ----------------------
void mouseDragged( ) {
  if( ( keyPressed ) && ( key == SHIFT ) ) {
    tx = px + mouseX - hx;
    ty = py + mouseY - hy;
  } else if( ( keyPressed ) && ( key == 'z' ) ) {
    sf = sp - 2.0 * ( mouseY - hy ) / width;
  } else {
    ry = ey + hy - mouseY;
    rx = ex + hx - mouseX;
  }
}

// --------------------- O R I E N T ----------------------
void orient() {
  translate( float( tx + width / 2 ), float( ty + height / 2 ), 0);
  rotateX( float(ry) * PI / 180.0 );
  rotateZ( float(rx) * PI / 180.0 );
  scale( float(sf), float(sf), float(sf) );
}