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

SoftwareSerial BT(3, 2); // RX, TX

Servo servo1, servo2, servo3, servo4;

int cur1, cur2, cur3, cur4;

void forceHomePose() {
  servo1.write(50);
  servo2.write(0);
  servo3.write(140);
  servo4.write(10);

  delay(1200);

  cur1 = 50; cur2 = 0; cur3 = 140; cur4 = 10;
}

void moveServoSmooth(Servo &s, int &cur, int target, int step, int dly) {
  if (cur == target) return;

  int dir = (target > cur) ? 1 : -1;
  step = abs(step);

  while (cur != target) {
    cur += dir * step;
    if ((dir > 0 && cur > target) || (dir < 0 && cur < target)) cur = target;
    s.write(cur);
    delay(dly);
  }
}

void setHomePoseSmooth() {
  moveServoSmooth(servo1, cur1, 50, 4, 20);
  moveServoSmooth(servo2, cur2, 0,  4, 20);
  moveServoSmooth(servo3, cur3, 140,4, 20);
  moveServoSmooth(servo4, cur4, 10, 4, 20);
}

void action1() {
  for (int i = 0; i <= 100; i += 4) {
    int a3 = map(i, 0, 100, 140, 0);
    servo3.write(a3);
    cur3 = a3;
    delay(10);
  }
  for (int i = 0; i <= 100; i += 4) {
    int a2 = map(i, 0, 100, 0, 120);
    int a3 = map(i, 0, 100, 0, 60);
    servo2.write(a2);
    servo3.write(a3);
    cur2 = a2;
    cur3 = a3;
    delay(10);
  }

  delay(2000);
  setHomePoseSmooth();
}

void action2() {
  int start2 = cur2;
  int start3 = cur3;
  int start4 = cur4;

  for (int t = 0; t <= 100; t += 4) {
    int a2 = map(t, 0, 100, start2, 80);
    int a3 = map(t, 0, 100, start3, 60);
    int a4 = map(t, 0, 100, start4, 60);
    servo2.write(a2);
    servo3.write(a3);
    servo4.write(a4);
    cur2 = a2;
    cur3 = a3;
    cur4 = a4;
    delay(10);
  }

  moveServoSmooth(servo1, cur1, 180, 4, 30);
  moveServoSmooth(servo1, cur1, 0,   4, 30);
  moveServoSmooth(servo1, cur1, 180, 4, 30);
  moveServoSmooth(servo1, cur1, 0,   4, 30);
  moveServoSmooth(servo1, cur1, 180, 4, 30);
  moveServoSmooth(servo1, cur1, 50,  4, 30);

  delay(2000);
  setHomePoseSmooth();
}

void action3() {
  for (int i = 0; i <= 100; i += 4) {
    int a3 = map(i, 0, 100, 140, 0);
    servo3.write(a3);
    cur3 = a3;
    delay(10);
  }
  delay(500);
  for (int i = 0; i <= 100; i += 4) {
    int a2 = map(i, 0, 100, 0, 120);
    servo2.write(a2);
    cur2 = a2;
    delay(5);
  }

  delay(2000);
  setHomePoseSmooth();
}

void setup() {
  Serial.begin(9600);
  BT.begin(9600);

  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);

  // Home after start
  forceHomePose();

  Serial.println("Ready");
}

void loop() {
  if (BT.available()) {
    char c = BT.read();
    if (c == '1') action1();
    else if (c == '2') action2();
    else if (c == '3') action3();
    else if (c == 'H' || c == 'h') setHomePoseSmooth();
  }

  if (Serial.available()) {
    BT.write(Serial.read());
  }
}