#include "HCPCA9685.h"
#include <SoftwareSerial.h>
#define  I2CAdd 0x40
HCPCA9685 HCPCA9685(I2CAdd);
SoftwareSerial controller(12, 13);
int bluetooth;
int bt;
void setup() {
  // put your setup code here, to run once:
  while (!Serial) {
    ;
  }
  pinMode(2,OUTPUT);
  controller.begin(38400);
  HCPCA9685.Init(SERVO_MODE);
  HCPCA9685.Sleep(false);
  Serial.begin(9600);
  servo(0, 0); servo(6, 90);
  servo(1, 90); servo(7, 0);
  servo(2, 0); servo(8, 10);

  servo(3, 90); servo(9, 93);
  servo(4, 90); servo(10, 90);
  servo(5, 100); servo(11, 90);

  servo(12, 87);
  servo(13, 80);
  delay(10);
 
}
void servo(int pin, int servoval) {
  servoval = map(servoval, 0, 180, 0, 455);
  HCPCA9685.Servo(pin, servoval);
}
void btread() {
  bluetooth = controller.read();

  if (!(bluetooth == -1 )) {
    bt = bluetooth;
  }
}
void loop() {
  // put your main code here, to run repeatedly:

  btread();
  Serial.println(bt);
  delay(100);

  if (bt == 1) {                    //hand wave
    Serial.println(1);
    servo(0, 180); servo(6, 90);
    servo(1, 90); servo(7, 0);
    servo(2, 90); servo(8, 10);

    servo(3, 87); servo(9, 87);
    servo(4, 90); servo(10, 90);
    servo(5, 102); servo(11, 90);

    servo(12, 87);
    servo(13, 90);
    delay(1200);

    servo(0, 180); servo(6, 90);
    servo(1, 00); servo(7, 0);
    servo(2, 90); servo(8, 10);

    servo(3, 87); servo(9, 87);
    servo(4, 90); servo(10, 90);
    servo(5, 102); servo(11, 90);

    servo(12, 87);
    servo(13, 90);
    delay(1200);

  } else if (bt == 3) {                 //block
    Serial.println(3);
    servo(0, 90); servo(6, 0);
    servo(1, 90); servo(7, 0);
    servo(2, 80); servo(8, 90);

    servo(3, 90); servo(9, 93);
    servo(4, 50); servo(10, 130);
    servo(5, 80); servo(11, 80);

    servo(12, 110);
    servo(13, 90);
  } else if (bt == 4) {                 //front punch
    Serial.println(4);
    servo(0, 90); servo(6, 0);
    servo(1, 90); servo(7, 0);
    servo(3, 105); servo(9, 105);
    servo(4, 35); servo(10, 150);
    servo(5, 45); servo(11, 60);
    servo(12, 80);
    servo(13, 110);

    servo(2, 0); servo(8, 90);
    delay(1500);
    servo(2, 90); servo(8, 90);
    delay(1500);
    
  } else if (bt == 2) {                 //push up
    Serial.println(2);
    for (int i = 0; i < 90; i++) {

      servo(0, 70); servo(6, 13);

      servo(1, i); servo(7, 90 - i);
      servo(2, ((90 - i) + 20) - 10); servo(8, 90 - i);

      servo(3, 90); servo(9, 90);
      servo(4, 90); servo(10, 90);
      servo(5, 100); servo(11, 90);

      servo(12, 70);
      servo(13, 90);
      delay(10);
    }
    for (int i = 0; i < 90; i++) {

      servo(0, 70); servo(6, 13);

      servo(1, 90 - i); servo(7, i);
      servo(2, i); servo(8, i);

      servo(3, 90); servo(9, 90);
      servo(4, 90); servo(10, 90);
      servo(5, 100); servo(11, 90);

      servo(12, 70);
      servo(13, 90);
      delay(10);
    }
  } else if (bt == 5) {                 //stand stright
    Serial.println(5);
    servo(0, 0); servo(6, 90);
    servo(1, 90); servo(7, 0);
    servo(2, 0); servo(8, 10);

    servo(3, 90); servo(9, 93);
    servo(4, 90); servo(10, 90);
    servo(5, 100); servo(11, 90);

    servo(12, 87);
    servo(13, 80);
    delay(10);
    delay(10);
  } else if (bt == 6) {                 //walk put in
    Serial.println(6);
    digitalWrite(2,HIGH);
  }else{
    digitalWrite(2,LOW);
  }

}