// updated 6.2.2019
// a collaboration by Kevin Li, Minh Ngo Duc, Brandon Ritter, and Thomas Trzpit

#include <Encoder.h>
#include <math.h>

/////////////////////
//  STATE MACHINE  //
/////////////////////

enum state_enum {START, LEADER_INPUT, FOLLOWER_HAPTIC, FOLLOWER_INPUT, ENDGAME, REALIGN};

////////////////////
//      PINS      //
////////////////////
//PLAYER ONE PANTOGRAPH
const int P1_M1_ENCODER_A = 2 ;
const int P1_M1_ENCODER_B = 16 ;
const int P1_M1_PWM1 = 5 ;
const int P1_M1_PWM2 = 6 ;
const int P1_M5_ENCODER_A = 3 ;
const int P1_M5_ENCODER_B = 4 ;
const int P1_M5_PWM1 = 10;
const int P1_M5_PWM2 = 11;
const int P1_button = 14;     // the number of the pushbutton pin

//PLAYER TWO PANTOGRAPH
const int P2_M1_ENCODER_A = 18 ;
const int P2_M1_ENCODER_B = 17 ;
const int P2_M1_PWM1 = 12;
const int P2_M1_PWM2 = 7;
const int P2_M5_ENCODER_A = 20; //need to figure out PCINT
const int P2_M5_ENCODER_B = 19; //need to figure out PCINT
const int P2_M5_PWM1 = 8;
const int P2_M5_PWM2 = 9;
const int P2_button = 15;

/////////////////////////
//      CONSTANTS      //
/////////////////////////
const float L1 = 130.0; //mm
const float L2 = 150.0; //mm
const float L3 = 150.0; //mm
const float L4 = 130.0; //mm
const float L5 = 85.0; //mm
const float CAPSTAN_RATIO = 13.5;
const float TORQUE_STALL = 0.0141; //Nm
const float MIN_DUTY = 10; //analog value /255
const float Q1_HOME = 60;//51.0; //deg
const float Q5_HOME = 120;//129.0; //deg
const int CPR = 48; //counts per rotation of encoder
const float offsetCenterX = 42.5;
const float offsetCenterY = -217.2;// -185.0;
const float tStep = 0.05; //[s]
const int max_dist = 50;
const int printRate = 5;

/////////////////////////////////
//      RUNTIME VARIABLES      //
/////////////////////////////////
int P1_controlsDir[2] = {0, 0};
int P1_controlsPwr[2] = {0, 0};
int P2_controlsDir[2] = {0, 0};
int P2_controlsPwr[2] = {0, 0};

float jacobian1[2][2] = {{0, 0}, {0, 0}};
float jacobian2[2][2] = {{0, 0}, {0, 0}};
float P1_q1 = 0;
float P1_q5 = 0;
float P2_q1 = 0;
float P2_q5 = 0;

float P1_pos[2] = {0, 0};
float P1_vel[2] = {0, 0};
float P1_posGraphics[2] = {0, 0}; //[mm]
float P1_velGraphics[2] = {0, 0};

float P2_pos[2] = {0, 0};
float P2_vel[2] = {0, 0};
float P2_posGraphics[2] = {0, 0}; //[mm]
float P2_velGraphics[2] = {0, 0};

//intialize output forces
float P1_f[2] = {0, 0};
float P2_f[2] = {0, 0};

// button states:
boolean P1_button_state = LOW;
boolean P2_button_state = LOW;

// game related
int curr_leader = 1; // player that is currently setting pattern
state_enum state = START;
const int START_LVL = 3;
const int MAX_LVL = 8;
int curr_lvl = START_LVL;
int sequence[MAX_LVL] =  {0, 0, 0, 0, 0, 0, 0, 0};
boolean listening = true;
int curr_idx = 0;
int follower_idx = 0;
boolean mustReturnHome = false;
int haptic_counter = 0;
int simon_wall_dir[4][2] = {{ -1, 0}, {0, 1}, {1, 0}, {0, -1}};
boolean game_lost = false;
boolean follower_plays = false;
int curr_player = curr_leader; 
int wall_num = 0; //0 is no hit, 1 is right wall, 2 is bottom wall, 3 is left wall, 4 is top wall
int is_game_over = 0; //0 is not game over, 1 is players win and level up, 2 is players win at max level, 3 is players lose

// Joystick rendering
float P1_joystickX;
float P1_joystickY;
float P2_joystickX;
float P2_joystickY;

Encoder P1_M1_Enc(P1_M1_ENCODER_A, P1_M1_ENCODER_B);
Encoder P1_M5_Enc(P1_M5_ENCODER_A, P1_M5_ENCODER_B);
Encoder P2_M1_Enc(P2_M1_ENCODER_A, P2_M1_ENCODER_B);
Encoder P2_M5_Enc(P2_M5_ENCODER_A, P2_M5_ENCODER_B);
int loopCounter = 0;

////////////////////////////////
//      FUNCTION HEADERS      //
////////////////////////////////
void updatePosition();
void updateControls(float F_x, float F_y, float jacobian[2][2], boolean numberOne);
void printControls();
void motorCommand(int M1Dir, int M1Pwr, int M5Dir, int M5Pwr, int M1_PWM1, int M1_PWM2, int M5_PWM1, int M5_PWM2);
void calcJacobian(float q1, float q5, boolean numberOne);
void printJacobian();
float cosd(float ang);
float sind(float ang);
int sgn(int val);
float mapFloat(float input, float map1L, float map1R, float map2L, float map2R);
void printToProcessing();
int wallCheck(int player); // returns wall number hit, if zero, did not hit
void drawJoysticks();
bool isHome(int player);
void home_force(int player);
void stopMotors(int player);
void resetGame(int toLevel);

/////////////////////
//      SETUP      //
/////////////////////
void setup() {
  Serial.begin(38400);
  // outputs
  pinMode (P1_M1_PWM1, OUTPUT);
  pinMode (P1_M1_PWM2, OUTPUT);
  pinMode (P1_M5_PWM1, OUTPUT);
  pinMode (P1_M5_PWM2, OUTPUT);
  pinMode (P2_M1_PWM1, OUTPUT);
  pinMode (P2_M1_PWM2, OUTPUT);
  pinMode (P2_M5_PWM1, OUTPUT);
  pinMode (P2_M5_PWM2, OUTPUT);
  // buttons
  pinMode (P1_button , INPUT);
  pinMode (P2_button , INPUT);

  stopMotors(1);
  stopMotors(2);
}

/////////////////////////
//      MAIN LOOP      //
/////////////////////////
void loop() {
  updatePosition();
  //printPosition();
  P1_button_state = digitalRead(P1_button);
  P2_button_state = digitalRead(P2_button);

  //update jacobian based on current configuration
  updateJacobian(P1_q1, P1_q5, true);
  updateJacobian(P2_q1, P2_q5, false);

  //Serial.print("current state number: ");
  //Serial.println(state);

  switch (state) {

    case START:
      if ((P1_button_state && curr_leader == 1) || (P2_button_state && curr_leader == 2)) { //did leader click
        state = LEADER_INPUT;
        curr_player = curr_leader; 
        mustReturnHome = false;
//        Serial.print("current leader: ");
//        Serial.print(curr_leader);
//        Serial.print("\t");
//        Serial.print("current level: ");
//        Serial.println(curr_lvl);
      }
      break;

    case LEADER_INPUT:
      //if we've reached max sequence length and returned home, transition to follower states
      if (sequence[curr_lvl - 1] != 0 && isHome(curr_leader)) {
        haptic_counter = 0;
        wall_num = 0;
        state = FOLLOWER_HAPTIC;
      }
      //else continue saving the sequence
      else {
        wall_num = wallCheck(curr_leader);
        if (!mustReturnHome &&  wall_num != 0) {
          sequence[curr_idx++] = wall_num;
          mustReturnHome = true;

          //Serial.print("wall_num: ");
          //Serial.println(wall_num);
        }

        if (mustReturnHome && isHome(curr_leader)) {
          mustReturnHome = false;
        }
      }
      home_force(curr_leader);
      break;

    case FOLLOWER_HAPTIC:
      {
        //Serial.println("FOLLOWER_HAPTIC");
        curr_player = (curr_leader == 1) ? 2 : 1;
        if (curr_leader == 1) {
          P1_f[0] = 0;
          P1_f[1] = 0;
        } else {
          P2_f[0] = 0;
          P2_f[1] = 0;
        }

        int idx = haptic_counter / 200;  // FLAG tbd what this delay should be
        if (idx/2 == curr_lvl) {
          mustReturnHome = false;
          state = FOLLOWER_INPUT;
          //Serial.println("exiting follower_haptic");
          if (curr_leader == 1) { // follower is 2
            P2_f[0] = 0;
            P2_f[1] = 0;
          } else {                // follower is 1
            P1_f[0] = 0;
            P1_f[1] = 0;
          }
        } else {
          if(idx%2 == 0){
            int replay_wall_num = sequence[idx/2];
            float f_simon = 1.0;
  
            if (curr_leader == 1) { // follower is 2
              P2_f[0] = f_simon * simon_wall_dir[replay_wall_num - 1][0];
              P2_f[1] = f_simon * simon_wall_dir[replay_wall_num - 1][1];
            } else {   // follower is 1
              P1_f[0] = f_simon * simon_wall_dir[replay_wall_num - 1][0];
              P1_f[1] = f_simon * simon_wall_dir[replay_wall_num - 1][1];
            }
          } else {
            home_force(curr_leader == 1 ? 2 : 1);
          }
        }
        haptic_counter++;
      }
      break;

    case FOLLOWER_INPUT:
      //Serial.println("entering follower input");
      
      if ((P2_button_state && curr_leader == 1) || (P1_button_state && curr_leader == 2)) {
        follower_plays = true;
        //Serial.println("follower is playing");
      }

      if (follower_plays) {
        home_force(curr_leader == 1 ? 2 : 1);
        wall_num = curr_leader == 1 ? wallCheck(2) : wallCheck(1);

        if (!mustReturnHome && wall_num != 0) {
          //Serial.print("wall hit: ");
          //Serial.println(hit_wall);

          if (sequence[follower_idx] != wall_num) { // check if current follower idx is the hit wall, if not we lose, otherwise we check next
            state = ENDGAME;
            wall_num = 0; 
            game_lost = true; 
            stopMotors(1);
            stopMotors(2);
          } else {
            follower_idx++;
            mustReturnHome = true;
          }
        }

        if ((follower_idx == curr_lvl) && isHome(curr_leader == 1 ? 2 : 1)) { // if completed sequence successfully, then go to the end game 
            state = ENDGAME;
            wall_num = 0;
            stopMotors(1);
            stopMotors(2);
        }

        if (mustReturnHome && isHome(curr_leader == 1 ? 2 : 1)) {
          mustReturnHome = false;
        }
      }
      break;

    case ENDGAME:
      //Serial.println("end game");
      //Serial.println(game_lost);
      if (!game_lost) {
        if (curr_lvl == MAX_LVL) {
          is_game_over = 2; //pair wins at max level
        } else {
          is_game_over = 1; //pair wins and levels up
          state = REALIGN; 
          resetGame(curr_lvl+1); //reset to next level
        }
      } else {
        is_game_over = 3; //pair loses
      }
      break;

    case REALIGN:
      P1_M1_Enc.write(0); 
      P1_M5_Enc.write(0); 
      P1_q1 = Q1_HOME;
      P1_q5 = Q5_HOME;

      P2_M1_Enc.write(0); 
      P2_M5_Enc.write(0);  
      P2_q1 = Q1_HOME;
      P2_q5 = Q5_HOME;
      is_game_over = 4; //prompt to realign
      if (P1_button_state && P2_button_state) {
        state = START; 
        is_game_over = 0;
      }
      break;
  }

  // end game logic


  drawJoysticks();

  //send force
  updateControls(P1_f[0], P1_f[1], jacobian1, true);
  updateControls(P2_f[0], P2_f[1], jacobian2, false);

  //send motor command
  motorCommand(P1_controlsDir[0], P1_controlsPwr[0],
               P1_controlsDir[1], P1_controlsPwr[1],
               P1_M1_PWM1, P1_M1_PWM2,
               P1_M5_PWM1, P1_M5_PWM2);

  motorCommand(P2_controlsDir[0], P2_controlsPwr[0],
               P2_controlsDir[1], P2_controlsPwr[1],
               P2_M1_PWM1, P2_M1_PWM2,
               P2_M5_PWM1, P2_M5_PWM2);

  //send to processing
  printToProcessing();
  //#numberOne

  //update loop counter
  loopCounter = loopCounter + 1;
}





/////////////////////////
//      FUNCTIONS      //
/////////////////////////
void updatePosition() {
  //read encoder values
  int P1_M1_counts = P1_M1_Enc.read();
  int P1_M5_counts = P1_M5_Enc.read();

  //convert to motor pulley angle then to sector pulley angle
  P1_q1 = -(360.0 * P1_M1_counts / 48.0) / CAPSTAN_RATIO + Q1_HOME;
  P1_q5 = -(360.0 * P1_M5_counts / 48.0) / CAPSTAN_RATIO + Q5_HOME;

  //read encoder values
  int P2_M1_counts = P2_M1_Enc.read();
  int P2_M5_counts = P2_M5_Enc.read();

  //convert to motor pulley angle then to sector pulley angle
  P2_q1 = -(360.0 * P2_M1_counts / 48.0) / CAPSTAN_RATIO + Q1_HOME;
  P2_q5 = -(360.0 * P2_M5_counts / 48.0) / CAPSTAN_RATIO + Q5_HOME;
}

void printPosition() {
  Serial.print("P1 Position: [");
  Serial.print(P1_pos[0]);
  Serial.print("\t");
  Serial.print(P1_pos[1]);
  Serial.print("]");
  Serial.print("\t");
  Serial.print(P1_q1);
  Serial.print("\t");
  Serial.print(P1_q5);

  Serial.print("\t P2 Position: [");
  Serial.print(P2_pos[0]);
  Serial.print("\t");
  Serial.print(P2_pos[1]);
  Serial.print("]");
  Serial.print("\t");
  Serial.print(P2_q1);
  Serial.print("\t");
  Serial.println(P2_q5);
}

void updateControls(float F_x, float F_y, float jacobian[2][2], boolean numberOne) {
  //torque at base link pulleys, converted to [Nm]
  float trq1 = (jacobian[0][0] * F_x + jacobian[1][0] * F_y) / 1000.0;
  float trq5 = (jacobian[0][1] * F_x + jacobian[1][1] * F_y) / 1000.0;

  //torque from motors [Nm]
  float trq_motor1 = trq1 / CAPSTAN_RATIO;
  float trq_motor5 = trq5 / CAPSTAN_RATIO;

  //save sign and convert trq to abs value
  if (numberOne) {
    P1_controlsDir[0] = sgn(trq_motor1);
    P1_controlsDir[1] = sgn(trq_motor5);
  } else {
    P2_controlsDir[0] = sgn(trq_motor1);
    P2_controlsDir[1] = sgn(trq_motor5);
  }
  trq_motor1 = abs(trq_motor1);
  trq_motor5 = abs(trq_motor5);

  //constrain to max torque (stall torque of motor) and adjust ratio to match

  trq_motor1 = constrain(trq_motor1, 0, TORQUE_STALL);
  trq_motor5 = constrain(trq_motor5, 0, TORQUE_STALL);

  //convert to PWM power, mapped  out of 255
  if (numberOne) {
    P1_controlsPwr[0] = (int)mapFloat(trq_motor1, 0, TORQUE_STALL, MIN_DUTY, 255);
    P1_controlsPwr[1] = (int)mapFloat(trq_motor5, 0, TORQUE_STALL, MIN_DUTY, 255);
  } else {
    P2_controlsPwr[0] = (int)mapFloat(trq_motor1, 0, TORQUE_STALL, MIN_DUTY, 255);
    P2_controlsPwr[1] = (int)mapFloat(trq_motor5, 0, TORQUE_STALL, MIN_DUTY, 255);
  }
}

void printControls() {
  Serial.print("PANTO 1 Motor1: [");
  Serial.print(P1_controlsDir[0]);
  Serial.print("\t");
  Serial.print(P1_controlsPwr[0]);
  Serial.print("] \t");
  Serial.print("Motor5: [");
  Serial.print(P1_controlsDir[1]);
  Serial.print("\t");
  Serial.print(P1_controlsPwr[1]);
  Serial.print("]");

  Serial.print("PANTO 2 Motor1: [");
  Serial.print(P2_controlsDir[0]);
  Serial.print("\t");
  Serial.print(P2_controlsPwr[0]);
  Serial.print("] \t");
  Serial.print("Motor5: [");
  Serial.print(P2_controlsDir[1]);
  Serial.print("\t");
  Serial.print(P2_controlsPwr[1]);
  Serial.println("]");



}

void motorCommand(int M1Dir, int M1Pwr, int M5Dir, int M5Pwr, int M1_PWM1, int M1_PWM2, int M5_PWM1, int M5_PWM2) {
  //set motor1
  if (M1Dir > 0) {
    analogWrite(M1_PWM1, M1Pwr);
    analogWrite(M1_PWM2, 0);
  }
  else if (M1Dir < 0) {
    analogWrite(M1_PWM1, 0);
    analogWrite(M1_PWM2, M1Pwr);
  }
  else {
    analogWrite(M1_PWM1, 0);
    analogWrite(M1_PWM2, 0);
  }

  //set motor5
  if (M5Dir > 0) {
    analogWrite(M5_PWM1, M5Pwr);
    analogWrite(M5_PWM2, 0);
  }
  else if (M5Dir < 0) {
    analogWrite(M5_PWM1, 0);
    analogWrite(M5_PWM2, M5Pwr);
  }
  else {
    analogWrite(M5_PWM1, 0);
    analogWrite(M5_PWM2, 0);
  }
}

void updateJacobian(float q1, float q5, boolean numberOne) {
  //calculate intermediate points
  float P1[2] = {0, 0};
  float P5[2] = { -L5, 0};

  float P2[2] = {L1 * cosd(q1), L1 * sind(q1)};
  float P4[2] = {L4 * cosd(q5) - L5, L4 * sind(q5)};

  float d = sqrt(pow((P2[0] - P4[0]), 2) + pow((P2[1] - P4[1]), 2));
  float b = abs((pow(L2, 2) - pow(L3, 2) + pow(d, 2)) / (2 * d));

  float Ph[2] = {P2[0] + b / d*(P4[0] - P2[0]),
                 P2[1] + b / d*(P4[1] - P2[1])
                };

  float h = sqrt(pow(L2, 2) - pow(b, 2));

  float P3[2] = {Ph[0] + h / d*(P4[1] - P2[1]),
                 Ph[1] - h / d*(P4[0] - P2[0])
                };
  if (numberOne) {
    P1_pos[0] = P3[0] + offsetCenterX;
    P1_pos[1] = P3[1] + offsetCenterY;
  } else {
    P2_pos[0] = P3[0] + offsetCenterX;
    P2_pos[1] = P3[1] + offsetCenterY;
  }


  //calculate jacobian
  float d1_x2 = -L1 * sind(q1);
  float d1_y2 = L1 * cosd(q1);
  float d5_x4 = -L4 * sind(q5);
  float d5_y4 = L4 * cosd(q5);

  float d1_y4, d1_x4, d5_y2, d5_x2 = 0;

  float d1_d = ((P4[0] - P2[0]) * (d1_x4 - d1_x2) + (P4[1] - P2[1]) * (d1_y4 - d1_y2)) / d;
  float d5_d = ((P4[0] - P2[0]) * (d5_x4 - d5_x2) + (P4[1] - P2[1]) * (d5_y4 - d5_y2)) / d;

  float d1_b = d1_d  - (d1_d * (pow(L2, 2) - pow(L3, 2) + pow(d, 2)) / (2 * pow(d, 2)));
  float d5_b = d5_d  - (d5_d * (pow(L2, 2) - pow(L3, 2) + pow(d, 2)) / (2 * pow(d, 2)));

  float d1_h = -b * d1_b / h;
  float d5_h = -b * d5_b / h;

  float d1_yh = d1_y2 + (d1_b * d - d1_d * b) / pow(d, 2) * (P4[1] - P2[1]) + b / d * (d1_y4 - d1_y2);
  float d5_yh = d5_y2 + (d5_b * d - d5_d * b) / pow(d, 2) * (P4[1] - P2[1]) + b / d * (d5_y4 - d5_y2);

  float d1_xh = d1_x2 + (d1_b * d - d1_d * b) / pow(d, 2) * (P4[0] - P2[0]) + b / d * (d1_x4 - d1_x2);
  float d5_xh = d5_x2 + (d5_b * d - d5_d * b) / pow(d, 2) * (P4[0] - P2[0]) + b / d * (d5_x4 - d5_x2);

  float d1_y3 = d1_yh - h / d * (d1_x4 - d1_x2) - (d1_h * d - d1_d * h) / pow(d, 2) * (P4[0] - P2[0]);
  float d5_y3 = d5_yh - h / d * (d5_x4 - d5_x2) - (d5_h * d - d5_d * h) / pow(d, 2) * (P4[0] - P2[0]);

  float d1_x3 = d1_xh + h / d * (d1_y4 - d1_y2) + (d1_h * d - d1_d * h) / pow(d, 2) * (P4[1] - P2[1]);
  float d5_x3 = d5_xh + h / d * (d5_y4 - d5_y2) + (d5_h * d - d5_d * h) / pow(d, 2) * (P4[1] - P2[1]);
  if (numberOne) {
    jacobian1[0][0] = d1_x3;
    jacobian1[0][1] = d5_x3;
    jacobian1[1][0] = d1_y3;
    jacobian1[1][1] = d5_y3;
  } else {
    jacobian2[0][0] = d1_x3;
    jacobian2[0][1] = d5_x3;
    jacobian2[1][0] = d1_y3;
    jacobian2[1][1] = d5_y3;
  }
}

void printJacobian() {
  Serial.println("PANTO 1 Jacobian:");
  Serial.print(jacobian1[0][0], 5);
  Serial.print("\t");
  Serial.print(jacobian1[0][1], 5);
  Serial.print("\n");
  Serial.print(jacobian1[1][0], 5);
  Serial.print("\t");
  Serial.print(jacobian1[1][1], 5);

  Serial.println("PANTO 2 Jacobian:");
  Serial.print(jacobian2[0][0], 5);
  Serial.print("\t");
  Serial.print(jacobian2[0][1], 5);
  Serial.print("\n");
  Serial.print(jacobian2[1][0], 5);
  Serial.print("\t");
  Serial.println(jacobian2[1][1], 5);
}

void printToProcessing() {
  //constrain to graphics space first
  P1_posGraphics[0] = constrain(P1_posGraphics[0], -max_dist, max_dist);
  P1_posGraphics[1] = constrain(P1_posGraphics[1], -max_dist, max_dist);
  P2_posGraphics[0] = constrain(P2_posGraphics[0], -max_dist, max_dist);
  P2_posGraphics[1] = constrain(P2_posGraphics[1], -max_dist, max_dist);
  //print at limited rate
  if (loopCounter % printRate == 0) {
    Serial.print(P1_posGraphics[0], 1);
    Serial.print(",");
    Serial.print(P1_posGraphics[1], 1);
    Serial.print(",");
    Serial.print(P2_posGraphics[0], 1);
    Serial.print(",");
    Serial.print(P2_posGraphics[1], 1);
    Serial.print(",");
    Serial.print(curr_player);
    Serial.print(",");
    Serial.print(wall_num);
    Serial.print(",");
    Serial.println(is_game_over);
  }
}

float cosd(float ang) {
  ang = ang * PI / 180;
  return cos(ang);
}

float tand(float ang) {
  ang = ang * PI / 180;
  return tan(ang);
}

float sind(float ang) {
  ang = ang * PI / 180;
  return sin(ang);
}

int sgn(float val) {
  if (val < 0) return -1;
  if (val == 0) return 0;
  return 1;
}

float mapFloat(float input, float map1L, float map1R, float map2L, float map2R) {
  return (input - map1L) / (map1R - map1L) * (map2R - map2L) + map2L;
}

void home_force(int player) {
  //  //BEGIN RETURNING JOYSTICK
  float kJoystick = 0.02; //[units?]
  float thres = 5;
  
  if (player == 1) {
    //joystick returning force for P1
    float P1_rPos = sqrt(pow(P1_pos[0], 2) + pow(P1_pos[1], 2));
    float P1_totalF = 0;

    if (P1_rPos > thres) {
      P1_totalF = kJoystick * (P1_rPos);
      P1_f[0] = -(P1_pos[0] / P1_rPos) * P1_totalF;
      P1_f[1] = -(P1_pos[1] / P1_rPos) * P1_totalF;
    } else {
      P1_f[0] = 0;
      P1_f[1] = 0;
    }
  }

  if (player == 2) {
    //joystick returning force for P2
    float P2_rPos = sqrt(pow(P2_pos[0], 2) + pow(P2_pos[1], 2));
    float P2_totalF = 0;

    if (P2_rPos > thres) {
      P2_totalF = kJoystick * (P2_rPos);
      P2_f[0] = -(P2_pos[0] / P2_rPos) * P2_totalF;
      P2_f[1] = -(P2_pos[1] / P2_rPos) * P2_totalF;
      //
    } else {
      P2_f[0] = 0;
      P2_f[1] = 0;  
    }
  }
}

int wallCheck (int curr_player) {
  // assume we're listening for wall collisions
  // if x is greater than x_wall, then hit wall 1, etc for all 4 walls
  // then record wall and place in correct position in sequence
  // check if hitting wall
  int x_wall = max_dist-5;
  int y_wall = max_dist-5;

  // if player 1 is player:
  if (curr_player == 1) {
    if (P1_pos[0] < -x_wall) {
      return 1;
    } else if (P1_pos[0] > x_wall) {
      return 3;
    } else if (P1_pos[1] < - y_wall) {
      return 4;
    } else if (P1_pos[1] > y_wall) {
      return 2;
    }
  }

  // check wall collisions of player 2
  if (curr_player == 2) {
    if (P2_pos[0] < -x_wall) {
      return 1;
    } else if (P2_pos[0] > x_wall) {
      return 3;
    } else if (P2_pos[1] < - y_wall) {
      return 4;
    } else if (P2_pos[1] > y_wall) {
      return 2;
    }
  }

  return 0;
}

bool shouldBeListening (int curr_leader) {
  float close_enough_thres = 2;
  if (curr_leader == 1) {
    float P1_rPos = sqrt(pow(P1_pos[0], 2) + pow(P1_pos[1], 2));
    return P1_rPos < close_enough_thres;
  }

  if (curr_leader == 2) {
    float P2_rPos = sqrt(pow(P2_pos[0], 2) + pow(P2_pos[1], 2));
    return P2_rPos < close_enough_thres;
  }
  return false;
}

void drawJoysticks() {
  //BEGIN JOYSTICK GRAPHICS
  float P1_scaling = 1;
  float P1_joystickDB = 5; //[mm]

  if (sqrt(pow(P1_pos[0], 2) + pow(P1_pos[1], 2)) > P1_joystickDB) {
    P1_joystickX = P1_pos[0];
    P1_joystickY = P1_pos[1];
  }
  else {
    P1_joystickX = 0;
    P1_joystickY = 0;
  }
  P1_posGraphics[0] = P1_scaling * P1_joystickX;
  P1_posGraphics[1] = P1_scaling * P1_joystickY;
  /*
  P1_velGraphics[0] = P1_scaling * P1_joystickX;
  P1_velGraphics[1] = P1_scaling * P1_joystickY;
  P1_posGraphics[0] = P1_posGraphics[0] + P1_velGraphics[0] * tStep;
  P1_posGraphics[1] = P1_posGraphics[1] + P1_velGraphics[1] * tStep;
  */

  //END JOYSTICK 1 GRAPHICS

  //BEGIN JOYSTICK 2 GRAPHICS

  float P2_scaling = 1;
  float P2_joystickDB = 5; //[mm]


  if (sqrt(pow(P2_pos[0], 2) + pow(P2_pos[1], 2)) > P2_joystickDB) {
    P2_joystickX = P2_pos[0];
    P2_joystickY = P2_pos[1];
  }
  else {
    P2_joystickX = 0;
    P2_joystickY = 0;
  }
  
  P2_posGraphics[0] = P2_scaling * P2_joystickX;
  P2_posGraphics[1] = P2_scaling * P2_joystickY;
  /*
  P2_velGraphics[0] = P2_scaling * P2_joystickX;
  P2_velGraphics[1] = P2_scaling * P2_joystickY;
  P2_posGraphics[0] = P2_posGraphics[0] + P2_velGraphics[0] * tStep;
  P2_posGraphics[1] = P2_posGraphics[1] + P2_velGraphics[1] * tStep;
*/
  //END JOYSTICK 2 GRAPHICS
}

boolean isHome(int player) {
  float homeRadius = 30;
  if (player == 1) {
    return (sqrt(P1_pos[0] * P1_pos[0] + P1_pos[1] * P1_pos[1]) < homeRadius);

  } else {
    return (sqrt(P2_pos[0] * P2_pos[0] + P2_pos[1] * P2_pos[1]) < homeRadius);
  }
}

void stopMotors(int player) {
  if (player == 1)
  {
    P1_f[0] = 0;
    P1_f[1] = 0;
  }
  else if (player == 2)
  {
    P2_f[0] = 0;
    P2_f[1] = 0;
  }
}

void resetGame(int toLevel) {
  curr_lvl = toLevel;
  curr_idx = 0;
  follower_idx = 0;
  game_lost = false;
  mustReturnHome = false;
  follower_plays = false;
  memset (sequence, 0, sizeof(sequence));
  stopMotors(1); 
  stopMotors(2); 
}
