/*
 * Testing out 'ServoEasing' library to smooth servo movements
 */

//#include <Servo.h>
#include <FastLED.h> //****Added for WS2812 led****
#include <ServoEasing.hpp> //ServoEasing library

//uncomment for digital servos in the Shoulder and Elbow
//that use a range of 900ms to 2100ms
//#define DIGITAL_RANGE

//Dimmensions of the arm 'bones' (inches)
const float A = 5.75;
const float B = 7.375;


//****Added for LEDS****
#define NUM_LEDS 1
#define DATA_PIN 2
CRGB leds[NUM_LEDS];


//Servo PWM output pins
#define Base_pin 3
#define Shoulder_pin 5
#define Elbow_pin 6
#define Wrist_pin 9
#define Gripper_pin 10
#define WristR_pin 11

//Radians to Degrees constant
const float rtod = 57.295779;

//Arm Speed Variables
float Speed = 1.0;
int sps = 3;

//Joints as Servo objects
ServoEasing Elb;
ServoEasing Shldr;
ServoEasing Wrist;
ServoEasing Base;
ServoEasing Gripper;
ServoEasing WristR;

//Arm Current Pos
float X = 4;
float Y = 4;
float Z = 90;
int G = 90;
float WA = 0;
int WR = 96;

//Arm temp pos
float tmpx = 4;
float tmpy = 4;
float tmpz = 90;
int tmpg = 90;
float tmpwa = 0;
int tmpwr = 96;

//'Arm' function including Inverse Kinematics to control the arm
int Arm(float x, float y, float z, int g, float wa, int wr)
{
  float M = sqrt((y*y)+(x*x));
  if(M <= 0)
    return 1;
  float A1 = atan(y/x);
  if(x <= 0)
    return 1;
  float A2 = acos((A*A-B*B+M*M)/((A*2)*M));
  float Elbow = acos((A*A+B*B-M*M)/((A*2)*B));
  float Shoulder = A1 + A2;
  Elbow = Elbow * rtod;
  Shoulder = Shoulder * rtod;
  if((int)Elbow <= 0 || (int)Shoulder <= 0)
    return 1;
  float Wris = abs(wa - Elbow - Shoulder) - 90;
  
#ifdef DIGITAL_RANGE
  Elb.writeMicroseconds(map(180 - Elbow, 0, 180, 900, 2100  ));
  Shldr.writeMicroseconds(map(Shoulder, 0, 180, 900, 2100));
#else
  //Elb.startEaseTo(180 - Elbow);
  //Shldr.startEaseTo(Shoulder);
  ServoEasing::ServoEasingNextPositionArray[2] = 180 - Elbow;
  ServoEasing::ServoEasingNextPositionArray[1] = Shoulder;
#endif

  //Wrist.startEaseTo(180 - Wris);
  //Base.startEaseTo(z);
  //WristR.startEaseTo(wr);
  //Gripper.startEaseTo(g);
  ServoEasing::ServoEasingNextPositionArray[3] = 180 - Wris;
  ServoEasing::ServoEasingNextPositionArray[0] = z;
  ServoEasing::ServoEasingNextPositionArray[5] = wr;
  ServoEasing::ServoEasingNextPositionArray[4] = g;
  setEaseToForAllServosSynchronizeAndStartInterrupt(15);


  X = tmpx;
  Y = tmpy;
  Z = tmpz;
  WA = tmpwa;
  G = tmpg;
  
  return 0; 
}

void setup()
{
  Serial.begin(9600);
  Serial.println("");
  Serial.println("******Serial started******");
  
  Base.attach(Base_pin);
  Base.setSpeed(20);
  Base.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Base Servo attached");
  
  Shldr.attach(Shoulder_pin);
  Shldr.setSpeed(20);
  Shldr.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Shoulder Servo attached");
  
  Elb.attach(Elbow_pin);
  Elb.setSpeed(20);
  Elb.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Elbow Servo attached");
  
  Wrist.attach(Wrist_pin);
  Wrist.setSpeed(30);
  Wrist.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Wrist Servo attached");
  
  Gripper.attach(Gripper_pin);
  Gripper.setSpeed(30);
  Gripper.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Gripper Servo attached");
  
  WristR.attach(WristR_pin);
  WristR.setSpeed(30);
  WristR.setEasingType(EASE_CUBIC_IN_OUT);
  Serial.println("Wrist Rotate Servo attached");
  
  Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);
  Serial.println("******Setup Finished******");


  //****Added for LED****
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS); 
  leds[0].setRGB(0, 25, 0); 
  FastLED.show();

  //****Added for button****
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
}

const float posDeltaX = 0.15;
const float posDeltaY = 0.15;
const float posDeltaZ = 2;
const float posDeltaWa = 2.5;
const int posDeltaG = 2.5;
const int posDeltaWr = 2.5;
long lastReferenceTime;
unsigned char action;

#define actionUp 119                // w
#define actionDown 115              // s
#define actionLeft 97               // a
#define actionRight 100             // d
#define actionRotCW 101             // e
#define actionRotCCW 113            // q
#define actionGripperOpen 114       // r
#define actionGripperClose 116      // t
#define actionWristUp 122           // z
#define actionWristDown 120         // x
#define actionWristRotCW 103        // g
#define actionWristRotCCW 102       // f



void loop()
{
  if(Serial.available() > 0)
  {
    // Read character
    action = Serial.read();
    if(action > 0)
    {
      // Set action
      switch(action)
      {
        case actionUp:
        tmpy += posDeltaY;
        break;
        
        case actionDown:
        tmpy -= posDeltaY;
        break;
        
        case actionLeft:
        tmpx += posDeltaX;
        break;
        
        case actionRight:
        tmpx -= posDeltaX;
        break;
        
        case actionRotCW:
        tmpz += posDeltaZ;
        break;
        
        case actionRotCCW:
        tmpz -= posDeltaZ;
        break;
        
        case actionGripperOpen:
        tmpg += posDeltaG;
        break;
        
        case actionGripperClose:
        tmpg -= posDeltaG;
        break;
        
        case actionWristUp:
        tmpwa += posDeltaWa;
        break;
        
        case actionWristDown:
        tmpwa -= posDeltaWa;
        break;
        
        case actionWristRotCW:
        tmpwr += posDeltaWr;
        break;
        
        case actionWristRotCCW:
        tmpwr -= posDeltaWr;
        break;
      }
      
      // Display position
      Serial.print("tmpx = "); Serial.print(tmpx, DEC); Serial.print("\ttmpy = "); Serial.print(tmpy, DEC); Serial.print("\ttmpz = "); Serial.print(tmpz, DEC); Serial.print("\ttmpg = "); Serial.print(tmpg, DEC); Serial.print("\ttmpwa = "); Serial.print(tmpwa, DEC); Serial.print("\ttmpwr = "); Serial.println(tmpwr, DEC);
      
      // Move arm
      if (Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr) == 1)
      {
        Serial.println("ERROR");
        leds[0].setRGB(25, 0, 0); 
        FastLED.show();
      }
      else
      {
        leds[0].setRGB(0, 25, 0); 
        FastLED.show();
      }
      
      /*
      //Pause for 100 ms between actions
      lastReferenceTime = millis();
      while(millis() <= (lastReferenceTime + 5)){};
      */
    }
  }
    //****Code to test input button on A0 pin****
    int button1Value = digitalRead(A0);
    if (button1Value == 0)
    {
      Serial.println("Home Button Pressed");
      tmpx = 4;
      tmpy = 4;
      tmpz = 90;
      tmpg = 90;
      tmpwa = 0;
      tmpwr = 96;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      do {
        leds[0].setRGB(0,0,75);
        FastLED.show();
      } while (ServoEasing::areInterruptsActive());

      leds[0].setRGB(0,75,0);
      FastLED.show();
    }

    //****Code to test input button on A1 pin****
    int button2Value = digitalRead(A1);
    if (button2Value == 0)
    {
      Serial.println("Action Button Pressed");

      leds[0].setRGB(0,0,75);
      FastLED.show();
      
      tmpx = 11.5;
      tmpy = 4.9;
      tmpz = 90;
      tmpg = 56;
      tmpwa = -60;
      tmpwr = 96;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpy = 3.85;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpg = 144;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());
      
      tmpy = 4.9;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpx = 6.7;
      tmpy = 3;
      tmpz = 180;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpy = 2.1;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpg = 56;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpy = 5.5;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      tmpx = 4;
      tmpy = 4;
      tmpz = 90;
      tmpg = 90;
      tmpwa = 0;
      tmpwr = 96;
      Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);

      while (ServoEasing::areInterruptsActive());

      leds[0].setRGB(0,75,0);
      FastLED.show();
    }
}