int totalObstacles = 20;

Vehicle vehicle = new Vehicle();
Obstacle[] obstacle = new Obstacle[totalObstacles];

float[] xObstacle = new float[totalObstacles];
float[] yObstacle = new float[totalObstacles];

// color palette stolen from Jared Tarbell of Levitated.net.  My own color palette will be arriving soon.
// rusted desert metal. winter, new mexico
color[] goodcolor = {#3a242b, #3b2426, #352325, #836454, #7d5533, #8b7352, #b1a181, #a4632e, #bb6b33, #b47249, #ca7239, #d29057, #e0b87e, #d9b166, #f5eabe, #fcfadf, #d9d1b0, #fcfadf, #d1d1ca, #a7b1ac, #879a8c, #9186ad, #776a8e};


float xMouse, yMouse;
float elevation, azimuth, twist, distance;


int border = 400;
// P5 ENVIRONMENTAL METHODS ++++++++++++++++++++++++++++++++++++++++++

void setup() {

  size(400,400);
  background(255);
  ellipseMode(CENTER_RADIUS);
  noStroke();
  smooth();

  elevation = radians(15.0f);
  azimuth = radians(0.0f);

  distance = 600.0f;

  vehicle = new Vehicle();
  // instantiate universe of particles
  for (int i=0; i<totalObstacles; i++) {
    obstacle[i] = new Obstacle(i);
  }
}

float xmag, ymag = 0;
float newXmag, newYmag = 0; 

void loop(){
  beginCamera();
  perspective(60.0f, (float)width / (float)height, 1.0f, 500);
  translate(0.0f, 0.0f, -distance);

  twist = radians(xMouse / 2.0f);
  azimuth = radians((300 - yMouse) / 2.0f);
  
  rotateY(elevation);
  rotateX(-azimuth);
  rotateZ(-twist);
  endCamera();

  xMouse -= (xMouse - (width/2 - mouseX)) * .2f;
  yMouse -= (yMouse - (height/2 - mouseY)) * .2f;
   
  background(0);
  vehicle.exist();
  for (int i=0; i<totalObstacles; i++) {
    obstacle[i].exist();
  }
}


class Vehicle {
  float x = 0;
  float y = 0;
  float xv, yv;
  float xf, yf;
  float dtf, dtv, atf, atv, dtc, atc, olddtc;
  float rad;
  float turnSpeed = 3;
  float maxSpeed = 7;
  float theta;
  float angle;
  float angleDelta;
  float speed;
  int colorVar = int(random(15));
  int turnCounter;
  
  boolean turn;
  
  Vehicle (){
  
  }
  
  void exist(){
    findVelocity();
    checkObstacles();
    checkBorder();
    move();
    render();
  }
  
  void findVelocity(){
    if (keyPressed){
      if (key == LEFT){
        if (angleDelta < 6){
          angleDelta += (2 * speed/6);
        } else {
          angleDelta = 6;
        }
        angle += angleDelta;
      } else if (key == RIGHT){
        if (angleDelta > -6){
          angleDelta -= (2 * speed/6);
        } else {
          angleDelta = -6;
        }
        angle += angleDelta;
      }
    } else {
      if (angleDelta < 0){
        angleDelta += .5;
      } else if (angleDelta > 0){
        angleDelta -= .5;
      } else {
        angleDelta += 0;
      }
      if (abs(angleDelta) < .5){
        angleDelta = 0;
      }
      angle += angleDelta;
      speed -= (speed - maxSpeed) * .2;
      turnSpeed -= (turnSpeed - 0) * .2;
    }

    theta = (-(angle * PI))/180;
    
    xv = cos(theta) * speed;
    yv = sin(theta) * speed;
  }
  
  void checkObstacles(){
    for (int i=0; i<totalObstacles; i++){
      rad = obstacle[i].radius;
      dtf = obstacle[i].distFromFeeler;
      dtv = obstacle[i].distFromVehicle;
      atf = obstacle[i].angleToFeeler;
      atv = obstacle[i].angleToVehicle;
      
      if (dtf < rad){
        speed -= .9f;
        turnSpeed += 2.0f;
        if (atf - atv > 180){
          atf -= 360;
        } else if (atv - atf > 180){
          atv -= 360;
        }
	
	if (atf > atv){
	  angle -= turnSpeed;
	} else {
	  angle += turnSpeed;
        }
      }
      
      if (dtv < rad){
	//collision = true;
	//findObsHit(i);
      }
    }
  }
  
  void checkBorder(){
    olddtc = dtc;
    dtc = findDistance(x, y, 0, 0);
    atc = findAngle(x, y, 0, 0);
    
    if(dtc > border){
      turn = true;
    }
    
    if (turn){
      if (dtc - olddtc > -5){
        turnSpeed += 3.0f;
        if (atc - angle > 180){
          atc -= 360;
        } else if (angle - atc > 180){
          angle -= 360;
        }
      
        if (atc > angle){
          angle -= turnSpeed;
        } else {
	  angle += turnSpeed;
        }
      } else {
        turn = false;
      }
    }
  }
  
  void move(){
    x += xv;
    y += yv;
    
    xf = x + (xv * speed * 1.5);
    yf = y + (yv * speed * 1.5);
  }
  


  void render(){
    push();
    translate(x, y, 0);
    fill(255);
    rotateZ(radians(-angle));
    box(12);

    translate(10,0,0);
    box(10);
    translate(-20,0,0);
    box(10);
    pop();
  }
}






class Obstacle{
  float x = random(-border * .9f, border * .9f);
  float y = random(-border * .9f, border * .9f);
  
  float distFromVehicle;
  float distFromFeeler;
  float angleToVehicle;
  float angleToFeeler;
  float distToCursor;
  int colorVar = (int)random(4, 10);
  float radius = random(30,50);
  
  int index;
  
  boolean mouseOver = false;
  
  Obstacle(int I){
    index = I;
  }
  
  void exist(){
    info();
    move();
    render();
  }
  
  void info(){
    distFromVehicle = findDistance(x, y, vehicle.x, vehicle.y);
    distFromFeeler = findDistance(x, y, vehicle.xf, vehicle.yf);
    angleToFeeler = findAngle(x, y, vehicle.xf, vehicle.yf);
    angleToVehicle = findAngle(x, y, vehicle.x, vehicle.y);
    distToCursor = findDistance(x, y, mouseX, mouseY);
  }
  
  void move(){
    xObstacle[index] = x;
    yObstacle[index] = y;
  }
  
  void render(){
    for (int i=0; i<8; i++){
      push();
      translate(x, y, (i-4)*4);
      fill(goodcolor[colorVar]);
      noStroke();
      ellipse(0, 0, radius * .6, radius * .6);
      pop();
    }
  
    
  }
}









float findDistance(float x1, float y1, float x2, float y2){
  float xd = x1 - x2;
  float yd = y1 - y2;
  
  float d = sqrt(xd*xd + yd*yd);
  return d;
}

float findAngle(float x1, float y1, float x2, float y2){
  float xd = x1 - x2;
  float yd = y1 - y2;
  
  float theta = atan2(yd,xd);
  //float angle = 360 - (180 + (-(180 * theta) / Math.PI));
  float angle = (-(180 * theta) / PI);
  return angle;
}