// ----------------------------------------------------------------------------------------------------------------
//                                           Vehicle
// ----------------------------------------------------------------------------------------------------------------

class Vehicle
{
  // Variables
  int i_myrotationDirection = 1, i_mypwrSwitch = 1, i_mypivotSwitch = 1;
  float f_myfirstX, f_myfirstY, f_mysecondX, f_mysecondY, f_myfirstwheelSpeed, f_mysecondwheelSpeed, f_mywheelTotal = 0, f_myaxelLength = 0, f_mywheelDiameter, f_mywheelWidth, f_myrtreadY, f_myltreadY;

  // Constructor (requires x and y coordinates, 
  Vehicle (float f_firstX, float f_firstY, float f_axelLength, float f_wheelDiameter, float f_wheelWidth, float f_firstwheelspeedSource, float f_secondwheelspeedSource)
 {
   f_myfirstX = f_firstX;
   f_myfirstY = f_firstY;
   f_myaxelLength = f_axelLength;
   f_mywheelDiameter = f_wheelDiameter;
   f_mywheelWidth = f_wheelWidth;
   f_myfirstwheelSpeed = f_firstwheelspeedSource;
   f_mysecondwheelSpeed = f_secondwheelspeedSource;
 }
   
// ---------------------------------------------------------------------------------------------------------------- calculate axel coordinates 
 
  void update()
  {
    // is the vehicle on?
    if (i_mypwrSwitch == 1) 
    {
      // which pivot is on?
      if (i_mypivotSwitch == 1)
      {
        // calculate axel position from left (first coordinates)
        // increment the angle the wheel is rotated by firstwheelSpeed
        f_mywheelTotal += i_myrotationDirection*f_myfirstwheelSpeed;
        f_mywheelTotal = f_mywheelTotal%360;
        // calculate the x coordinate of the second point
        f_mysecondX = f_myfirstX + ( f_myaxelLength * sin(radians(f_mywheelTotal+90)));
        // calculate the y coordinate of the second point
        f_mysecondY = f_myfirstY + ( f_myaxelLength * cos(radians(f_mywheelTotal+90)));
        
        // the tread moves according the the speed of the first wheel
        f_myrtreadY += .5*f_myfirstwheelSpeed;
        if (f_myrtreadY < (-1*f_mywheelDiameter/2)) f_myrtreadY = f_mywheelDiameter/2;
        if (f_myrtreadY > (f_mywheelDiameter/2)) f_myrtreadY = -1*f_mywheelDiameter/2;
      } else
      {
        // calculate axel position from right (second coordinates)
        // increment the angle the wheel is rotated by secondwheelSpeed
        f_mywheelTotal += i_myrotationDirection*f_mysecondwheelSpeed;
        f_mywheelTotal = f_mywheelTotal%360;
        // calculate the x coordinate of the second point
        f_myfirstX = f_mysecondX - ( f_myaxelLength * sin(radians(f_mywheelTotal+90)));
        // calculate the y coordinate of the second point
        f_myfirstY = f_mysecondY - ( f_myaxelLength * cos(radians(f_mywheelTotal+90)));
        
        // the tread moves according the the speed of the second wheel
        f_myltreadY += .5*f_mysecondwheelSpeed;
        if (f_myltreadY < (-1*f_mywheelDiameter/2)) f_myltreadY = f_mywheelDiameter/2;
        if (f_myltreadY > (f_mywheelDiameter/2)) f_myltreadY = -1*f_mywheelDiameter/2;
      }      
    }

// ---------------------------------------------------------------------------------------------------------------- Draw Vehicle
    
      // draw the axel as a line
      stroke(0, 0, 100);
      line(f_myfirstX, f_myfirstY, f_mysecondX, f_mysecondY); 
    
      // draw two wheels at the end points
      //fill(0, 0, 0);
      noFill();
      rectMode(CENTER);
      // Right Wheel
      pushMatrix();
      translate(f_mysecondX, f_mysecondY);
      rotate(radians(-1*(f_mywheelTotal)));
      rect(0, 0, f_mywheelWidth, f_mywheelDiameter);
      popMatrix();
  
      // Left Wheel
      pushMatrix();
      translate(f_myfirstX, f_myfirstY);
      rotate(radians(-1*(f_mywheelTotal)));
      rect(0, 0, f_mywheelWidth, f_mywheelDiameter);
      popMatrix(); 
      
      // Algorithim for placing objects  keep this for future reference
      //f_myboxX = f_myfirstX + ((sqrt(sq(f_myaxelLength*1.1/2)+sq(f_myaxelLength/2))) * sin(radians(f_mywheelTotal+90)+atan((f_myaxelLength*1.1/2)/(f_myaxelLength/2))));
      //f_myboxY = f_myfirstY + ((sqrt(sq(f_myaxelLength*1.1/2)+sq(f_myaxelLength/2))) * cos(radians(f_mywheelTotal+90)+atan((f_myaxelLength*1.1/2)/(f_myaxelLength/2))));
      
      //f_myboxX = placeX(f_myaxelLength/2, f_myaxelLength*1.1/2);
      //f_myboxY = placeY(f_myaxelLength/2, f_myaxelLength*1.1/2);
      
      // Vehicle Box
      
      // the width of the box fits between the wheels, and the length is rectangular and can be thought to extend out from the axel
      pushMatrix();
      translate(placeX(f_myaxelLength/2, f_myaxelLength*1.1/2), placeY(f_myaxelLength/2, f_myaxelLength*1.1/2));
      rotate(radians(-1*(f_mywheelTotal)));
      rect(0, 0, f_myaxelLength-f_mywheelWidth, f_myaxelLength*1.1);
      popMatrix();
      
      // Right Wheel Tread
      pushMatrix();
      translate(placeX(f_myaxelLength, f_myrtreadY), placeY(f_myaxelLength, f_myrtreadY));
      rotate(radians(-1*(f_mywheelTotal)));
      rect(0, 0, f_mywheelWidth, 0.1);
      popMatrix();
      
      // Left Wheel Tread
      pushMatrix();
      translate(placeX(0.1, f_myltreadY), placeY(0.1, f_myltreadY));
      //translate(-5.0, 0.0);
      rotate(radians(-1*(f_mywheelTotal)));
      rect(0, 0, f_mywheelWidth, 0.1);
      popMatrix();
       
  }
  
// ---------------------------------------------------------------------------------------------------------------- CRUCIAL METHODS
  
  // calculate what the new x position of the original item x coordinate
  float placeX(float f_itemX, float f_itemY)
  {
    return (f_myfirstX + ((sqrt(sq(f_itemX)+sq(f_itemY))) * sin(radians(f_mywheelTotal+90)+atan(f_itemY/f_itemX))));   
  }
  // calculate what the new y position of the original item y coordinate
  float placeY(float f_itemX, float f_itemY)
  {
    return (f_myfirstY + ((sqrt(sq(f_itemX)+sq(f_itemY))) * cos(radians(f_mywheelTotal+90)+atan(f_itemY/f_itemX))));
  }
    
  // swith witch pivot is acitvated
  void switchPivot()
  {
    // change the wheel rotation so that it keeps the same GENERAL direction it was previously going in
    i_myrotationDirection *= -1;
    
    i_mypivotSwitch *= -1;
    
  }
  
  // return the angle the wheel is rotated (vital so that objects placed on vehicle keep same rotation
  float getrotationAngle()
  {
    return f_mywheelTotal;
  }
  
  // get the speed of the right wheel
  float getrightwheelSpeed()
  {
    return f_myfirstwheelSpeed;
  }
  // set the speed of the right wheel
  void setrightwheelSpeed(float f_newfirstwheelSpeed)
  {
    f_myfirstwheelSpeed = f_newfirstwheelSpeed;
  }
  // get the speed of the left wheel
  float getleftwheelSpeed()
  {
    return f_mysecondwheelSpeed;
  }
  // set the speed of the left wheel
  void setleftwheelSpeed(float f_newsecondwheelSpeed)
  {
    f_mysecondwheelSpeed = f_newsecondwheelSpeed;
  }

// ---------------------------------------------------------------------------------------------------------------- EXTRA MOTION CONTROL

  // change the total angle the wheel is rotated to a new angle (be cautious when using this, add witch switch is activated before calling method)
  void setrotationAngle(float f_newwheelTotal)
  {
    f_mywheelTotal = f_newwheelTotal;
  }
  
  // get the rotation direction of the axel (clockwise = 1, counterclockwise = -1)
  int getrotationDirection()
  {
    return i_myrotationDirection;
  }
  // set the rotation direction of the axel
  void setrotationDirection(int i_newrotationDirection)
  {
    i_myrotationDirection = i_newrotationDirection;
  }
  
  // tell me if the vehicle is on (1 = on, -1 = off)
  int getpwrSwitch()
  {
    return i_mypwrSwitch;
  }
  // set the vehicle power to on or off
  void setpwrSwitch(int i_newpwrSwitch)
  {
    i_mypwrSwitch = i_newpwrSwitch;
  }
  
  // get witch pivot is on (1 for first wheel, -1 for second)
  int getpivotSwitch()
  {
    return i_mypivotSwitch;
  }
  // set witch pivot is on
  void setpivotSwitch(int i_newpivotSwitch)
  {
    i_mypivotSwitch = i_newpivotSwitch;
  }
  
 // ---------------------------------------------------------------------------------------------------------------- CHANGE VEHICLE SIZE & WHEELS
  
  // get axel length
  float getaxelLength()
  {
    return f_myaxelLength;
  }
  // set axel length 
  void setaxelLength(float f_newaxelLength)
  {
    f_myaxelLength = f_newaxelLength;
  }
  
  // get wheel diameter
  float getwheelDiameter()
  {
    return f_mywheelDiameter;
  }
  // set wheel diameter
  void setwheelDiameter(float f_newwheelDiameter)
  {
    f_mywheelDiameter = f_newwheelDiameter;
  }
  
  // get wheel width
  float getwheelWidth()
  {
    return f_mywheelWidth;
  }
  // set wheel width
  void setwheelWidth(float f_newwheelWidth)
  {
   f_mywheelWidth = f_newwheelWidth;
  } 

// ---------------------------------------------------------------------------------------------------------------- CHANGE FIRST & SECOND POINTS
  
  // get the x coordinate of the first point
  float getfirstX()
  {
    return f_myfirstX;
  }
  // set the x coordinate of the first point
  void setfirstX(float f_newfirstX)
  {
    f_myfirstX = f_newfirstX;
  }
  // get the y coordinate of the first point
  float getfirstY()
  {
    return f_myfirstY;
  }
  // set the y coordinate of the first point
  void setfirstY(float f_newfirstY)
  {
    f_myfirstY = f_newfirstY;
  }
  
  // get the x coordinate of the second point
  float getsecondX()
  {
    return f_mysecondX;
  }
  // set the x coordinate of the second point
  void setsecondX(float f_newsecondX)
  {
    f_mysecondX = f_newsecondX;
  }
  // get the y coordinate of the second point
  float getsecondY()
  {
    return f_mysecondY;
  }
  // set the y coordinate of the second point
  void setsecondY(float f_newsecondY)
  {
    f_mysecondY = f_newsecondY;
  }
      
}

// ----------------------------------------------------------------------------------------------------------------
//                      VehicleSystem
// ----------------------------------------------------------------------------------------------------------------