// 3 D  V E H I C L E S
// Yanni Loukissas (adapted from code by Simon Greenwold)

// GLOBAL CONSTANTS

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

color PEN;
boolean RUN;

// Number of vehicles on the image
int TRAIL_WIDTH = 20;
final int NUM_VEHICLES = X_SIZE/int(TRAIL_WIDTH);
final int VEHICLE_WIDTH = 5;
final int VEHICLE_LENGTH = 8;
final color VEHICLE_COLOR = #FF0000;
int VEHICLE_COUNT = 0;

// Increasing this increases the speed they travel
// also makes it harder for them to turn sharply
final float SPEED_MULT = 12;

// 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 = false;
// 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.2; // 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;

int count = 0;// keeps trak of how much time has passed
Point[] arrImgPoint;
Point ImgPoint;

// --------------- S E T U P -------------------------------
// Initialize the starting conditions
// Randomly assign values to all cells
void setup()
{
  colorMode(RGB, 100);
  size(X_SIZE, Y_SIZE);
  bgImage = loadImage(imageName);
  bgImage2 = loadImage(imageName2);
  RUN = false;
  makeVehicles();
  framerate(FRAME_RATE);
  PEN = color(100,100,100);// default pen color
}

// --------------- L O O P -------------------------------
void loop()
{
  orient();
  adjustPen();
  paintwithlight();
  background(100,100,100);

  // press q to launch vehicles
  if ( ( keyPressed ) && ( key == 'q' ) )  {
    RUN = true;
  }

  // once vehicles have been launched, update their states
  if (RUN){
    calculateNextVehicleStates();
    updateVehicleStates();
    drawVehicles();
    count++;
  }

  drawImage();
}

// ----------------------- 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;            // The color of the vehicle.
  boolean onEdge;                  // Have we hit an edge?
  int lifespan = 100;             //number of steps that vehicles move
  Point[] trail = new Point[lifespan*10]; //holds trail for vehicle
  int trailcount;
  float trailwidth;
  color shadowColor;
  int lifepoints;
  boolean live;
  float b;                         //brightness level between 0 and 1 at bgImage.get(x,y)

  // ------------------ V E H I C L E ---------------------------------
  // Constructor function. Tzhis is called each time a new vehicle object is
  // created. Randomly assigns state values.
  Vehicle(int aType) {

    live = true;
    lifepoints = 0;
    type = aType;
    
    x = VEHICLE_COUNT * TRAIL_WIDTH; 
    z = 0;
    y = 0;
    direction = 0;

    onEdge = false;
    VEHICLE_COUNT++;

    trailcount = 0;
    width = VEHICLE_WIDTH;
    length = VEHICLE_LENGTH;
    
    vehicleColor = color(random(70, 100), 0, 0);
    shadowColor = color(red(vehicleColor) - 60, red(vehicleColor) - 60, red(vehicleColor) - 60);
  }

  // ------------------ 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() {
    if (live) {
      x = nextX;
      y = nextY;
      b = (brightness(bgImage2.get((int)lSensorX, (int)lSensorY)) / 100)
      -(brightness(bgImage.get((int)lSensorX, (int)lSensorY)) / 100);
      z = (1 - b) * 100;
      if (b > .2) {
        lifespan += 100;
      }
    }

    if (trailcount < lifespan){
      trail[trailcount] = new Point(x,y,z);
      trailcount++;
      direction = nextDirection;
      updateSensorPositions();  // Update where the sensors are
    }
    else if ((X_SIZE < x || x < 0) || (Y_SIZE < y || y < 0)) {
      live = false;
    }
  }

  // ------------------ 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 = direction + PI;
      onEdge = false;
      return;
    }

    // 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;
    int t = 7;

    //stroke(vehicleColor);
    if (live) {
      stroke(color(0,50,red(vehicleColor)));
      fill(color(0,50,red(vehicleColor)));
    }
    else {
      stroke(color(0,0,0));
      noFill();
    }

    //connector
    line(ix, iy, iz, ix, iy, 0);

    //constructor
    push();
    
    translate(ix, iy, iz);
    rotate(direction);
    box(TRAIL_WIDTH, 1, 2);
    pop();

    //sensor
    ellipseMode(CENTER_DIAMETER);
    ellipse(ix, iy, 10, 10);

    // Graph
    if (trailcount > t) {
      for (int i = 0; i < (trailcount - t); i = i + t) {
        makeStruct(trail[i], trail[i + t]);
      }
    }

  }

  // --------------- M A K E  S T R U C T -------------------------
  void makeStruct(Point p1, Point p2) {
    Point[] pi = new Point[4];
    Point[] po = new Point[4];
    float bpoint;
    float x1, y1, z1;
    float x2, y2, z2;

    pi[0] = new Point(p1.x + TRAIL_WIDTH/2, p1.y, p1.z + TRAIL_WIDTH/2);
    pi[1] = new Point(p1.x - TRAIL_WIDTH/2, p1.y, p1.z + TRAIL_WIDTH/2);

    po[0] = new Point(p2.x + TRAIL_WIDTH/2, p2.y, p2.z + TRAIL_WIDTH/2);
    po[1] = new Point(p2.x - TRAIL_WIDTH/2, p2.y, p2.z + TRAIL_WIDTH/2);

    bpoint = brightness(shadowColor);

    x1 = (float)p1.x;
    y1 = (float)p1.y;
    z1 = (float)p1.z;

    x2 = (float)p2.x;
    y2 = (float)p2.y;
    z2 = (float)p2.z;

    //tests distance that vehicle moves as signal of darkess below
    if (distance(x1,y1,z1,x2,y2,z2) > bpoint/1.5) {
      stroke(vehicleColor);
      noFill();
      castShadows(false);
    }
    else {
      noStroke();
      fill(vehicleColor);
      castShadows(true);
    }

    // Building Shades
    beginShape(QUADS);
    vertex(float(po[0].x), float(po[0].y), float(po[0].z));
    vertex(float(pi[0].x), float(pi[0].y), float(pi[0].z));
    vertex(float(pi[1].x), float(pi[1].y), float(pi[1].z));
    vertex(float(po[1].x), float(po[1].y), float(po[1].z));
    endShape();
  }

  // ------------ C A S T  S H A D O W S -----------------------

  void castShadows(boolean shadowsOn) {
    color c;
    for (int i = (-TRAIL_WIDTH/2); i < (TRAIL_WIDTH/2); i++) {
      for (int ii = -1; ii > -TRAIL_WIDTH; ii--) {
        c = bgImage.get(int(x) + i, int(y) + ii);
 
        if ((c > shadowColor) && shadowsOn){ //don't overwrite darker shadows
          bgImage.set(int(x) + i, int(y) + ii, shadowColor);
        }
        else {
          bgImage.set(int(x) + i, int(y) + ii, c);
        } 
      }
    }
  }
}

// -------------- P O I N T  C L A S S -------------------------
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 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));
  }
}

// ------------------ 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));
  }
}

// --------------- 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() {
  image(bgImage,0,0);

  push();
  translate(0,0,-300);
  image(bgImage2,0,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 if( ( keyPressed ) && ( key == 'x' ) ) {
    ry = ey + hy - mouseY;
    rx = ex + hx - mouseX;
  }
}

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

// --------------- D I S T A N C E ----------------------
float distance(float x1, float y1, float z1, float x2, float y2, float z2) {
  float dist;
  float dx, dy, dz;
  dx = abs(x1 - x2);
  dy = abs(y1 - y2);
  dz = abs(z1 - z2);
  dist = sqrt(sq(dx) + sq(dy) + sq(dz));
  return dist;
}

// ------------- P A I N T  W I T H  L I G H T --------------
void paintwithlight() {
  //hold down "d" to ddraw.
  if( ( keyPressed ) && ( key == 'd' ) ) {
    for (int i = 0; i < 50; i++) {
      for (int ii = 0; ii < 50; ii++) {
        bgImage.set(mouseX + i, mouseY + ii, PEN);
      }
    }
  }
}

// ------------------------- P E N ---------------------------
void adjustPen() {
  // use numbers "1" through "9" to control brightness of PEN

  if ( ( keyPressed ) && ( key == '1' ) )  {
    PEN = color(0,0,0);
  }
  if ( ( keyPressed ) && ( key == '2' ) )  {
    PEN = color(10,10,10);
  }
  if ( ( keyPressed ) && ( key == '3' ) )  {
    PEN = color(20,20,20);
  }
  if ( ( keyPressed ) && ( key == '4' ) )  {
    PEN = color(30,30,30);
  }
  if ( ( keyPressed ) && ( key == '5' ) )  {
    PEN = color(40,40,40);
  }
  if ( ( keyPressed ) && ( key == '6' ) )  {
    PEN = color(50,50,50);
  }
  if ( ( keyPressed ) && ( key == '7' ) )  {
    PEN = color(60,60,60);
  }
  if ( ( keyPressed ) && ( key == '8' ) )  {
    PEN = color(70,70,70);
  }
  if ( ( keyPressed ) && ( key == '9' ) )  {
    PEN = color(80,80,80);
  }
  if ( ( keyPressed ) && ( key == '0' ) )  {
    PEN = color(100,100,100);
  }
}