//Based on the position of the hand received from the Raspberry PI, we actuate servo motors and LEDs corresponding to that position
//Added feedback
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <FastLED.h>

#define LED_PIN1    2 //Four Rows of RGB LEDs
#define LED_PIN2    3
#define LED_PIN3    4
#define LED_PIN4    5
#define NUM_LEDS    32

CRGB ledsr1[NUM_LEDS];
CRGB ledsr2[NUM_LEDS];
CRGB ledsr3[NUM_LEDS];
CRGB ledsr4[NUM_LEDS];

CRGB leds[NUM_LEDS];

Adafruit_PWMServoDriver myServo = Adafruit_PWMServoDriver();


int incomingByte = 0; // for incoming serial data




void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); // opens serial port, sets data rate to 9600 bps
  Serial.setTimeout(1);
  FastLED.addLeds<WS2812, LED_PIN1, GRB>(ledsr1, NUM_LEDS);
  FastLED.addLeds<WS2812, LED_PIN2, GRB>(ledsr2, NUM_LEDS);
  FastLED.addLeds<WS2812, LED_PIN3, GRB>(ledsr3, NUM_LEDS);
  FastLED.addLeds<WS2812, LED_PIN4, GRB>(ledsr4, NUM_LEDS);
  myServo.begin();
  myServo.setPWMFreq(50);
  delay(10);
}

void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    if (incomingByte == '1'){     //If the hand is located at the top left corner then actuate the first cylinder and first 8 Leds
      myServo.setPWM(0, 0, 250);         
      ledsr1[0] = CRGB(255, 225, 255);
      ledsr1[1] = CRGB(255, 225, 255);
      ledsr1[2] = CRGB(255, 225, 255);
      ledsr1[3] = CRGB(255, 225, 255);
      ledsr1[4] = CRGB(255, 225, 255);
      ledsr1[5] = CRGB(255, 225, 255);
      ledsr1[6] = CRGB(255, 225, 255);
      ledsr1[7] = CRGB(255, 225, 255);
      FastLED.show();
      Serial.print('1');
      Serial.flush();
    }
    else if (incomingByte == '2'){
      myServo.setPWM(1, 0, 250);
      ledsr1[8] = CRGB(255, 225, 255);
      ledsr1[9] = CRGB(255, 225, 255);
      ledsr1[10] = CRGB(255, 225, 255);
      ledsr1[11] = CRGB(255, 225, 255);
      ledsr1[12] = CRGB(255, 225, 255);
      ledsr1[13] = CRGB(255, 225, 255);
      ledsr1[14] = CRGB(255, 225, 255);
      ledsr1[15] = CRGB(255, 225, 255);
      FastLED.show();
      Serial.print('2');
      Serial.flush();      
    }
    else if (incomingByte == '3'){
      myServo.setPWM(2, 0, 250);
      ledsr1[16] = CRGB(255, 225, 255);
      ledsr1[17] = CRGB(255, 225, 255);
      ledsr1[18] = CRGB(255, 225, 255);
      ledsr1[19] = CRGB(255, 225, 255);
      ledsr1[20] = CRGB(255, 225, 255);
      ledsr1[21] = CRGB(255, 225, 255);
      ledsr1[22] = CRGB(255, 225, 255);
      ledsr1[23] = CRGB(255, 225, 255);
      FastLED.show();
      Serial.print('3');
      Serial.flush();  
    }
    else if (incomingByte == '4'){
      myServo.setPWM(3, 0, 250);
      ledsr1[24] = CRGB(255, 225, 255);
      ledsr1[25] = CRGB(255, 225, 255);
      ledsr1[26] = CRGB(255, 225, 255);
      ledsr1[27] = CRGB(255, 225, 255);
      ledsr1[28] = CRGB(255, 225, 255);
      ledsr1[29] = CRGB(255, 225, 255);
      ledsr1[30] = CRGB(255, 225, 255);
      ledsr1[31] = CRGB(255, 225, 255);
      Serial.flush();    
      Serial.print('4');               
      FastLED.show();
    }
    else if (incomingByte == '5'){
      myServo.setPWM(4, 0, 250);
      ledsr2[0] = CRGB(255, 225, 255);
      ledsr2[1] = CRGB(255, 225, 255);
      ledsr2[2] = CRGB(255, 225, 255);
      ledsr2[3] = CRGB(255, 225, 255);
      ledsr2[4] = CRGB(255, 225, 255);
      ledsr2[5] = CRGB(255, 225, 255);
      ledsr2[6] = CRGB(255, 225, 255);
      ledsr2[7] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('5');      
      FastLED.show();
    }
    else if (incomingByte == '6'){
      myServo.setPWM(5, 0, 250);
      ledsr2[8] = CRGB(255, 225, 255);
      ledsr2[9] = CRGB(255, 225, 255);
      ledsr2[10] = CRGB(255, 225, 255);
      ledsr2[11] = CRGB(255, 225, 255);
      ledsr2[12] = CRGB(255, 225, 255);
      ledsr2[13] = CRGB(255, 225, 255);
      ledsr2[14] = CRGB(255, 225, 255);
      ledsr2[15] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('6');      
      FastLED.show();
    }
    else if (incomingByte == '7'){
      myServo.setPWM(6, 0, 250);
      ledsr2[16] = CRGB(255, 225, 255);
      ledsr2[17] = CRGB(255, 225, 255);
      ledsr2[18] = CRGB(255, 225, 255);
      ledsr2[19] = CRGB(255, 225, 255);
      ledsr2[20] = CRGB(255, 225, 255);
      ledsr2[21] = CRGB(255, 225, 255);
      ledsr2[22] = CRGB(255, 225, 255);
      ledsr2[23] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('7');      
      FastLED.show();
    }
    else if (incomingByte == '8'){
      myServo.setPWM(7, 0, 250);
      ledsr2[24] = CRGB(255, 225, 255);
      ledsr2[25] = CRGB(255, 225, 255);
      ledsr2[26] = CRGB(255, 225, 255);
      ledsr2[27] = CRGB(255, 225, 255);
      ledsr2[28] = CRGB(255, 225, 255);
      ledsr2[29] = CRGB(255, 225, 255);
      ledsr2[30] = CRGB(255, 225, 255);
      ledsr2[31] = CRGB(255, 225, 255);
      Serial.flush();   
      Serial.print('8');      
      FastLED.show();
    }
    else if (incomingByte == '9'){
      myServo.setPWM(8, 0, 250);
      ledsr3[0] = CRGB(255, 225, 255);
      ledsr3[1] = CRGB(255, 225, 255);
      ledsr3[2] = CRGB(255, 225, 255);
      ledsr3[3] = CRGB(255, 225, 255);
      ledsr3[4] = CRGB(255, 225, 255);
      ledsr3[5] = CRGB(255, 225, 255);
      ledsr3[6] = CRGB(255, 225, 255);
      ledsr3[7] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('9');      
      FastLED.show();
    }
    else if (incomingByte == 'a'){
      
      myServo.setPWM(9, 0, 250);
      ledsr3[8] = CRGB(255, 225, 255);
      ledsr3[9] = CRGB(255, 225, 255);
      ledsr3[10] = CRGB(255, 225, 255);
      ledsr3[11] = CRGB(255, 225, 255);
      ledsr3[12] = CRGB(255, 225, 255);
      ledsr3[13] = CRGB(255, 225, 255);
      ledsr3[14] = CRGB(255, 225, 255);
      ledsr3[15] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('a');      
      FastLED.show();
    }
    else if (incomingByte == 'b'){
      myServo.setPWM(10, 0, 250);
      ledsr3[16] = CRGB(255, 225, 255);
      ledsr3[17] = CRGB(255, 225, 255);
      ledsr3[18] = CRGB(255, 225, 255);
      ledsr3[19] = CRGB(255, 225, 255);
      ledsr3[20] = CRGB(255, 225, 255);
      ledsr3[21] = CRGB(255, 225, 255);
      ledsr3[22] = CRGB(255, 225, 255);
      ledsr3[23] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('b');      
      FastLED.show();
    }
    else if (incomingByte == 'c'){
      myServo.setPWM(11, 0, 250);
      ledsr3[24] = CRGB(255, 225, 255);
      ledsr3[25] = CRGB(255, 225, 255);
      ledsr3[26] = CRGB(255, 225, 255);
      ledsr3[27] = CRGB(255, 225, 255);
      ledsr3[28] = CRGB(255, 225, 255);
      ledsr3[29] = CRGB(255, 225, 255);
      ledsr3[30] = CRGB(255, 225, 255);
      ledsr3[31] = CRGB(255, 225, 255);
      Serial.print('c');
      Serial.flush();  
      FastLED.show();
    }
    else if (incomingByte == 'd'){
      myServo.setPWM(12, 0, 250);
      ledsr4[0] = CRGB(255, 225, 255);
      ledsr4[1] = CRGB(255, 225, 255);
      ledsr4[2] = CRGB(255, 225, 255);
      ledsr4[3] = CRGB(255, 225, 255);
      ledsr4[4] = CRGB(255, 225, 255);
      ledsr4[5] = CRGB(255, 225, 255);
      ledsr4[6] = CRGB(255, 225, 255);
      ledsr4[7] = CRGB(255, 225, 255);
      Serial.flush();      
      Serial.print('d');        
      FastLED.show();
    }
    else if (incomingByte == 'e'){
      myServo.setPWM(13, 0, 250);
      ledsr4[8] = CRGB(255, 225, 255);
      ledsr4[9] = CRGB(255, 225, 255);
      ledsr4[10] = CRGB(255, 225, 255);
      ledsr4[11] = CRGB(255, 225, 255);
      ledsr4[12] = CRGB(255, 225, 255);
      ledsr4[13] = CRGB(255, 225, 255);
      ledsr4[14] = CRGB(255, 225, 255);
      ledsr4[15] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('e');
      FastLED.show();
    }
    else if (incomingByte == 'f'){
      myServo.setPWM(14, 0, 250);
      ledsr4[16] = CRGB(255, 225, 255);
      ledsr4[17] = CRGB(255, 225, 255);
      ledsr4[18] = CRGB(255, 225, 255);
      ledsr4[19] = CRGB(255, 225, 255);
      ledsr4[20] = CRGB(255, 225, 255);
      ledsr4[21] = CRGB(255, 225, 255);
      ledsr4[22] = CRGB(255, 225, 255);
      ledsr4[23] = CRGB(255, 225, 255);
      Serial.flush();  
      Serial.print('f');      
      FastLED.show();
    }
    else if (incomingByte == 'g'){
      myServo.setPWM(15, 0, 250);
      ledsr4[24] = CRGB(255, 225, 255);
      ledsr4[25] = CRGB(255, 225, 255);
      ledsr4[26] = CRGB(255, 225, 255);
      ledsr4[27] = CRGB(255, 225, 255);
      ledsr4[28] = CRGB(255, 225, 255);
      ledsr4[29] = CRGB(255, 225, 255);
      ledsr4[30] = CRGB(255, 225, 255);
      ledsr4[31] = CRGB(255, 225, 255); 
      Serial.flush();  
      Serial.print('g');      
      FastLED.show();
    }
    else if (incomingByte =='x'){
      myServo.setPWM(0, 0, 540);
      myServo.setPWM(1, 0, 540);
      myServo.setPWM(2, 0, 540);
      myServo.setPWM(3, 0, 540);
      myServo.setPWM(4, 0, 540);
      myServo.setPWM(5, 0, 540);
      myServo.setPWM(6, 0, 540);
      myServo.setPWM(7, 0, 540);
      myServo.setPWM(8, 0, 540);
      myServo.setPWM(9, 0, 540);
      myServo.setPWM(10, 0, 540);
      myServo.setPWM(11, 0, 540);
      myServo.setPWM(12, 0, 540);
      myServo.setPWM(13, 0, 540);
      myServo.setPWM(14, 0, 540);
      myServo.setPWM(15, 0, 540);

      for(int i=0; i<NUM_LEDS ;i++)
      {
        ledsr1[i] = CRGB(0, 0, 0);
        FastLED.show();
        }
       for(int i=0; i<NUM_LEDS ;i++)
      {
        ledsr2[i] = CRGB(0, 0, 0);
        FastLED.show();
        } 
        for(int i=0; i<NUM_LEDS ;i++)
      {
        ledsr3[i] = CRGB(0, 0, 0);
        FastLED.show();
        }
        for(int i=0; i<NUM_LEDS ;i++)
      {
        ledsr4[i] = CRGB(0, 0, 0);
        FastLED.show();
        }
      Serial.flush();   
      Serial.print('x');
       
        
  }
 }
  /*else 
  {
    
      myServo.setPWM(0, 0, 250);
      myServo.setPWM(1, 0, 540);
      myServo.setPWM(2, 0, 250);
      myServo.setPWM(3, 0, 540);
      myServo.setPWM(4, 0, 540);
      myServo.setPWM(5, 0, 250);
      myServo.setPWM(6, 0, 540);
      myServo.setPWM(7, 0, 250);
      myServo.setPWM(8, 0, 250);
      myServo.setPWM(9, 0, 540);
      myServo.setPWM(10, 0, 250);
      myServo.setPWM(11, 0, 540);    
      myServo.setPWM(12, 0, 540);                                     //myServo.setPWM(12, 0, 96); actual start     myServo.setPWM(12, 0, 250); limited start       myServo.setPWM(15, 0, 540); end
      myServo.setPWM(13, 0, 250);
      myServo.setPWM(14, 0, 540);  
      myServo.setPWM(15, 0, 250); 
     
      delay(2000);
      myServo.setPWM(0, 0, 540);
      myServo.setPWM(1, 0, 250);
      myServo.setPWM(2, 0, 540);
      myServo.setPWM(3, 0, 250); 
      myServo.setPWM(4, 0, 250);
      myServo.setPWM(5, 0, 540);
      myServo.setPWM(6, 0, 250);
      myServo.setPWM(7, 0, 540);
      myServo.setPWM(8, 0, 540);
      myServo.setPWM(9, 0, 250);
      myServo.setPWM(10, 0, 540);
      myServo.setPWM(11, 0, 250);   
      myServo.setPWM(12, 0, 250);
      myServo.setPWM(13, 0, 540);
      myServo.setPWM(14, 0, 250);  
      myServo.setPWM(15, 0, 540);   
                                                        //myServo.setPWM(servonum, 0, 700);
      delay(2000);

    
  } */
}