#include <MobaTools.h>
#include <Smoothed.h>

//Forward Kinematics
const float pi = 3.1415926;
const float L1 = 120; // Length between the first motor and the second motor (in cm)
const float L2 = 120; // Length between the second motor and the end of arm (in cm)
float y; 
float z; 
float q1; // Angle of the first motor (in radian)
float q1_deg; // Angle of the first motor (in degree)
float q2; // Angle of the second motor (in radian)
float q2_deg; // Angle of the second motor (in degree)
float q; // Sum of in radian (q1+q2), it called theta
float q_deg; // Sum of in degree (q1_deg + q2_deg)
float x_sq ; // X square
float y_sq ; // // Y square
float L1_sq ; // L1 square
float L2_sq ; // L2 square

//Sensor smoothing
Smoothed <float> shoulderSensor; 
Smoothed <float> elbowSensor;
Smoothed <float> baseSensor;



#define STEP_PIN_HIGH         54 // Step
#define DIR_PIN_HIGH          55 // Direction
#define ENABLE_PIN_HIGH       38

#define STEP_PIN_LOW         60 // Step
#define DIR_PIN_LOW          61 // Direction
#define ENABLE_PIN_LOW       56

#define STEP_PIN_RAIL         26 // Step
#define DIR_PIN_RAIL          28 // Direction
#define ENABLE_PIN_RAIL       24

#define XYspeed 5000

const int stepsPerRev = 400; 

MoToStepper stepperHigh (stepsPerRev, STEPDIR); //STEPDIR is for 2 pin stepper drivers
MoToStepper stepperLow (stepsPerRev, STEPDIR);
MoToStepper stepperRail (stepsPerRev, STEPDIR);

MoToStepper grabber(2048, FULLSTEP); 
int grabTarget = 500;
int grabMotion = 0;
const byte stPn[] = {11,6,5, 4};


float xPos = 105;
float yPos = 120;
float zPos = 120;



void setup() {

//Serial.begin(9600);

//grabber

grabber.attach( stPn[0],stPn[1],stPn[2],stPn[3] );
grabber.setSpeedSteps(4000);
grabber.setZero(); 
//grabber button
pinMode(44, INPUT_PULLUP);

//smoothing
shoulderSensor.begin(SMOOTHED_EXPONENTIAL, 10);
elbowSensor.begin(SMOOTHED_EXPONENTIAL, 10);
baseSensor.begin(SMOOTHED_EXPONENTIAL, 10);

//Stepper pins
stepperHigh.attach(STEP_PIN_HIGH, DIR_PIN_HIGH);
stepperLow.attach(STEP_PIN_LOW, DIR_PIN_LOW);
stepperRail.attach(STEP_PIN_RAIL, DIR_PIN_RAIL);

// Home Low Stepper
pinMode(ENABLE_PIN_LOW, OUTPUT);
digitalWrite(ENABLE_PIN_LOW, LOW);

stepperLow.setSpeed(XYspeed);
stepperLow.setRampLen(50);  

while (digitalRead(14)== HIGH) {
    stepperLow.rotate(1);
}

stepperLow.stop();
stepperLow.setZero();
stepperLow.writeSteps(-3720);
stepperLow.setZero(3480);

// Low Stepper Done


// Home High Stepper
pinMode(ENABLE_PIN_HIGH, OUTPUT);
digitalWrite(ENABLE_PIN_HIGH, LOW);

stepperHigh.setSpeed(XYspeed);
stepperHigh.setRampLen(50); 


while (digitalRead(3)== HIGH) {
    stepperHigh.rotate(-1);
}

stepperHigh.stop();
stepperHigh.setZero();
stepperHigh.writeSteps(1750);
stepperHigh.setZero(1750);
// Low Stepper Done


//Rail Stepper Home
pinMode(ENABLE_PIN_RAIL, OUTPUT);
digitalWrite(ENABLE_PIN_RAIL, LOW);

stepperRail.setSpeed(2500);
stepperRail.setRampLen(5); 


while (digitalRead(18)== HIGH) {
    stepperRail.rotate(1);
}

stepperRail.stop();
stepperRail.setZero(0);
stepperRail.writeSteps(-2750);
stepperRail.setSpeed(750);

 //Rail Stepper Done

 
}

void loop() {

float shoulderRead = analogRead(A5);
float elbowRead = analogRead(A10);

shoulderSensor.add(shoulderRead);
elbowSensor.add(elbowRead);


if ( analogRead(A9) < 524){
    xPos = xPos -5;             
} else if (analogRead(A9) > 575){
    xPos = xPos +5;           
  } 
  


  //assume the values of the angles
  q1_deg = map(shoulderSensor.get(), 133, 790, 0, 180);
  q2_deg = map(elbowSensor.get(), 973, 273, 0, 180);
  q_deg = q1_deg + q2_deg; 
  
  q1 = q1_deg*pi/180;// convert the angle from degree to radian
  q2 = q2_deg*pi/180;// convert the angle from degree to radian
  q = q_deg*pi/180; // convert the angle from degree to radian
  
  yPos = -L1*cos(q1) - L2*cos(q); // The expected value with respect to X axis 
  zPos = L1*sin(q1) + L2*sin(q);  // The expected value with respect to Y axis 




  xPos = max(xPos, 0);
  xPos = min(xPos, 5500);
  yPos = max(yPos, 70);
  yPos = min(yPos, 200);
  zPos = max(zPos, -90);
  zPos = min(zPos, 110);


 

  stepperRail.moveTo(-xPos); 

  moveToPos(yPos, zPos); //x position, y position, z position

  if (digitalRead(44) == 0){
    grabMotion = 0;
  } else {
    grabMotion = grabTarget;
  }
  grabber.moveTo(grabMotion);

}



void moveToPos(float y, float z) {

  float h = sqrt (y*y + z*z);

  float phi = atan(z/y) * (180 / 3.1415);

  float theta = acos((h/2)/120) * (180 / 3.1415);
  
  float a1 = phi + theta; // angle for first part of the arm
  float a2 = phi - theta; // angle for second part of the arm
  

  float lowSteps = -14400 -((a1/360) * -28800); 
  float highSteps = (a2/360) * -28800;
  

  stepperLow.moveTo(lowSteps);
  stepperHigh.moveTo(highSteps);

 /*Serial.print("low steps: ");
  Serial.print(lowSteps);
  Serial.print(" high steps: ");
  Serial.println(highSteps);

  Serial.print("low angle: ");
  Serial.print(a1);
  Serial.print(" high angle: ");
  */Serial.println(a2);
  
 //delayMicroseconds(750);

}