#include <MEncoder.h>
#include <Servo.h>

#define joyXPin A0
#define joyYPin A1
#define joy2XPin A2
#define joy2YPin A3

#define gearRatio 4.2 //Motor to large gear
#define cyclesPerRev 252.882 //Encoder cycles

#define AHipYawEncoder 20
#define BHipYawEncoder 21

#define AHipPitchEncoder 22
#define BHipPitchEncoder 23

#define AHipRollEncoder 24
#define BHipRollEncoder 25

#define hipYawMotorPin 9
#define hipPitchMotorPin 8
#define hipRollMotorPin 10

#define pitchSwitchPin 24

int joyValueX = 0;
int joyValueY = 0;
int joy2ValueX = 0;
int jou2ValueY = 0;

int hipPitchMotorSpeed = 0;
int hipYawMotorSpeed = 0;
int hipRollMotorSpeed = 0;

bool setupComplete = false;

Servo hipMotorYaw;
Servo hipMotorPitch;
Servo hipMotorRoll;

MEncoder hipYawEncoder(AHipYawEncoder, BHipYawEncoder, gearRatio, cyclesPerRev);
MEncoder hipPitchEncoder(AHipPitchEncoder, BHipPitchEncoder, gearRatio, cyclesPerRev);
MEncoder hipRollEncoder(AHipRollEncoder, BHipRollEncoder, gearRatio, cyclesPerRev);
/**
   Pin assignment
*/
void setup() {
  Serial.begin(9600);

  pinMode(hipYawMotorPin, OUTPUT);
  pinMode(hipPitchMotorPin, OUTPUT);
  pinMode(hipRollMotorPin, OUTPUT);


  hipMotorYaw.attach(hipYawMotorPin);
  hipMotorPitch.attach(hipPitchMotorPin);
  hipMotorRoll.attach(hipRollMotorPin);

  attachInterrupt(AHipYawEncoder, updateYawEncoder, CHANGE);
  attachInterrupt(AHipPitchEncoder, updatePitchEncoder, CHANGE);
  attachInterrupt(AHipRollEncoder, updateRollEncoder, CHANGE);

  Serial.println("START READING");
  while (!setupComplete) {
    Serial.println("SETUP STARTING");
    startup(); //Start home positioning
  }
}

/**
    Sets the home position by hitting limit switch twice
*/
void startup() {

  if (digitalRead(pitchSwitchPin) == 0) { //Motor engaged limit switch: At home position (Max rotation)

    Serial.println("RECALIBRATE");
    hipMotorPitch.write(30); //Go backwards
    delay(1000);

    while (!(digitalRead(pitchSwitchPin) == 0)) //While switch isn't engaged, go forward (Recalibration stage)
    {
      hipMotorPitch.write(120);
    }
    hipMotorPitch.write(90); //Stop motor
    Serial.println("DONE");

    setupComplete = true;
  }
  else //Motor has not engaged limit switch
  {
    for (int posPitch = 90; posPitch < 180; posPitch += 1)
    {
      if (digitalRead(pitchSwitchPin) == 0) {
        setupComplete = true;
        delay(1000);
        break;
      }
      hipMotorPitch.write(posPitch);
    }
  }
}
/**
   Handles manual control of all motors
*/
void loop()
{

  joyValueX = (map(analogRead(joyXPin), 0, 1023, 0, 180) - 10);
  joyValueY = (map(analogRead(joyYPin), 0, 1023, 0, 180) - 10);

  if (digitalRead(pitchSwitchPin) == 0)
  {
    Serial.println("Contact");
    if (joyValueY > 90) {
      hipMotorPitch.write(90);
    }
    else
    {
      hipMotorPitch.write(joyValueY);
    }
    hipMotorYaw.write(joyValueX);

  }
  else
  {
    hipMotorYaw.write(joyValueX);
    hipMotorPitch.write(joyValueY);
  }
}
/**
   Updates count (position) value for encoder object
*/
void encoderUpdater(MEncoder encoder)
{
  encoder.update();
}

void updateYawEncoder() {
  encoderUpdater(hipYawEncoder);
}
void updatePitchEncoder() {
  encoderUpdater(hipPitchEncoder);
}
void updateRollEncoder() {
  encoderUpdater(hipRollEncoder);
}