#include <SD.h>
#define SD_ChipSelectPin 10
#include <TMRpcm.h>
#include <pcmConfig.h>
#include <pcmRF.h>
#include <SPI.h>
TMRpcm cool;

#include <Servo.h>
#include <SoftwareSerial.h>

Servo head;
Servo l_hand;
Servo r_hand;

SoftwareSerial BT(5, 6); // RX, TX

// define sonar sensor's pins
int trig = 5;
int echo = 6;

// received data
byte val = "";

void setup() {
  Serial.begin(9600);     // for USB

    if (!SD.begin(SD_ChipSelectPin)) {
    Serial.println("SDFehler beim Start");
    return;
  }
  
  BT.begin(9600);         // for Bluetooth
  

  cool.speakerPin = 9;
  pinMode(8, OUTPUT);
  digitalWrite(8, HIGH);

  head.attach(2);
  l_hand.attach(7);
  r_hand.attach(4);


}

void standby() {
  head.write(90);
  int r_pos = 30;
  int l_pos = map(r_pos, 0, 180, 180, 0);
  l_hand.write(l_pos);
  r_hand.write(r_pos);
}

void hi() {
  head.write(90);
  for (int i = 30; i <= 170; i++) {
    r_hand.write(i);
    delay(5);
  }
  for (int i = 170; i >= 100; i--) {
    r_hand.write(i);
    delay(5);
  }
  for (int i = 100; i <= 170; i++) {
    r_hand.write(i);
    delay(5);
  }
  for (int i = 170; i >= 30; i--) {
    r_hand.write(i);
    delay(5);
  }
  standby();
}

void hands_up() {
  for (int i = 30; i <= 170; i++) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }
  delay(600);
  for (int i = 170; i >= 30; i--) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }
}

void weight_lift() {
  for (int i = 30; i <= 170; i++) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }

  for (int count = 0; count <= 4; count++) {
    for (int i = 170; i >= 60; i--) {
      int r_pos = i;
      int l_pos = map(r_pos, 0, 180, 180, 0);
      l_hand.write(l_pos);
      r_hand.write(r_pos);
      delay(5);
    }
    for (int i = 60; i <= 170; i++) {
      int r_pos = i;
      int l_pos = map(r_pos, 0, 180, 180, 0);
      l_hand.write(l_pos);
      r_hand.write(r_pos);
      delay(5);
    }
  }

  for (int i = 170; i >= 30; i--) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }
}

void excited() {
  return;
}

void look_left() {
  head.write(180);
}

void confused() {
  for (int count = 0; count <= 1; count++) {
    head.write(30);
    r_hand.write(170);
    delay(700);
    r_hand.write(30);
    head.write(120);
    l_hand.write(30);
    delay(700);
    l_hand.write(160);
  }
  standby();
}

void double_punch() {
  for (int i = 30; i >= 0; i--) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }
  delay(2000);
  int r_pos = 80;
  int l_pos = map(r_pos, 0, 180, 180, 0);
  l_hand.write(l_pos);
  r_hand.write(r_pos);
  delay(500);
  standby();
}

void r_upper_cut() {
  for (int i = 30; i <= 170; i++) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }

  for (int count = 0; count <= 4; count++) {
    for (int i = 170; i >= 60; i--) {
      r_hand.write(i);
      delay(1);
    }
    for (int i = 60; i <= 170; i++) {
      r_hand.write(i);
      delay(1);
    }
  }
  standby();
  delay(100);
}

void smash() {
  for (int i = 30; i <= 170; i++) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(5);
  }
  delay(2000);
  for (int i = 170; i >= 0; i--) {
    int r_pos = i;
    int l_pos = map(r_pos, 0, 180, 180, 0);
    l_hand.write(l_pos);
    r_hand.write(r_pos);
    delay(1);
  }
  delay(300);
  int r_pos = 180;
  int l_pos = map(r_pos, 0, 180, 180, 0);
  l_hand.write(l_pos);
  r_hand.write(r_pos);
  delay(1000);
  standby();
}

void eye_detect() {
  return;
}

void loop() {
  standby();

  if (Serial.available()) {
    val = Serial.read();
  } else if (BT.available()) {
    Serial.println("ERROR");
    val = BT.read();
  } else {
    return; 
  }

  if (val == 'h') {
    cool.setVolume(5);
    cool.play("abcd.wav");
    hi();
  } else if (val == 'p') {
    double_punch();
  } else if (val == 'u') {
    hands_up();
    delay(3000);
  } else if (val == 'l') {
    standby();
    look_left();
    delay(2000);
  } else if (val == 'U') {
    r_upper_cut();
    delay(2000);
  } else if (val == 's') {
    smash();
    delay(2000);
  }
}