#include <Servo.h>

bool repeatPlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int s1Pin = 11;         /* s1 servo */
int s2Pin = 10;         /* s2 servo */
int s3Pin = 9;          /* s3 servo */
int s4Pin = 6;          /* s4 servo */
int s5Pin = 5;          /* s5 servo */

int wDirPin = A0;       /* s1 - joystick1*/
int xDirPin = A1;       /* s2 - joystick1 */
int yDirPin = A3;       /* s3 - joystick2 */
int zDirPin = A2;       /* s4 - joystick2 */
int gDirPin = A4;       /* s5 - joystick3 */

int pinRecord = 2;      /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay   = 3;      /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13;  /* LED - indicates recording (light) or auto play mode (blink ones) */

const int buffSize = 512; /* Size of recording buffer */

int startS1 = 90;
int startS2 = 90;
int startS3 = 90;
int startS4 = 0;
int startS5 = 90;

int posS1 = 90;
int posS2 = 90;
int posS3 = 90;
int posS4 = 0;
int posS5 = 90;

int lastS1 = 90;
int lastS2 = 90;
int lastS3 = 90;
int lastS4 = 90;
int lastS5 = 90;

int minS1 = 0;
int maxS1 = 150;
int minS2 = 0;
int maxS2 = 150;
int minS3 = 0;
int maxS3 = 150;
int minS4 = 0;
int maxS4 = 150;
int minS5 = 0;
int maxS5 = 150;

const int countServo = 5;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servoS1;
Servo servoS2;
Servo servoS3;
Servo servoS4;
Servo servoS5;

void setup() {
  Serial.begin(115200);

  pinMode(wDirPin, INPUT);
  pinMode(xDirPin, INPUT);
  pinMode(yDirPin, INPUT);
  pinMode(zDirPin, INPUT);
  pinMode(gDirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servoS1.attach(s1Pin);
  servoS2.attach(s2Pin);
  servoS3.attach(s3Pin);
  servoS4.attach(s4Pin);
  servoS5.attach(s5Pin);

  StartPosition();

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatPlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatPlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatPlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dw = map(analogRead(wDirPin), 0, 1023, -5.0, 5.0);
  float dx = map(analogRead(xDirPin), 0, 1023, 5.0, -5.0);
  float dy = map(analogRead(yDirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zDirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gDirPin), 0, 1023, 5.0, -5.0);

  if (abs(dw) < 1.5) dw = 0;
  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posS1 += dw;
  posS2 += dx;
  posS3 += dy;
  posS4 += dz;
  posS5 += dg;

  if (play)
  {
    if (playPos >= recPos) {
      playPos = 0;

      if (repeatPlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData = false;

    while (!endOfData)
    {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x7000;
      endOfData = data & 0x8000;

      switch (servoNumber)
      {
        case 0x0000:
          posS1 = angle;
          break;

        case 0x1000:
          posS2 = angle;
          break;

        case 0x2000:
          posS3 = angle;
          break;

        case 0x3000:
          posS4 = angle;
          dz = posS4 - lastS4;
          break;

        case 0x4000:
          posS5 = angle;
          dg = posS5 - lastS5;
          break;
      }

      playPos++;
    }
  }

  if (posS1 > maxS1) posS1 = maxS1;
  if (posS2 > maxS2) posS2 = maxS2;
  if (posS3 > maxS3) posS3 = maxS3;
  if (posS4 > maxS4) posS4 = maxS4;
  if (posS5 > maxS5) posS5 = maxS5;

  if (posS1 < minS1) posS1 = minS1;
  if (posS2 < minS2) posS2 = minS2;
  if (posS3 < minS3) posS3 = minS3;
  if (posS4 < minS4) posS4 = minS4;
  if (posS5 < minS5) posS5 = minS5;

  servoS1.write(posS1);
  servoS2.write(posS2);
  servoS3.write(posS3);

  //  if (dg < -3.0) {
  //    posS4 = minS4;
  //    servoS4.write(posS4);
  //    Serial.println(posS4);
  //  }
  //  else if (dg > 3.0) {
  //    posS4 = maxS4;
  //    servoS4.write(posS4);
  //    Serial.println(posS4);
  //  }

  bool waitS4 = false;
  if (dz < 0) {
    posS4 = minS4;
    waitS4 = true;
  }
  else if (dz > 0) {
    posS4 = maxS4;
    waitS4 = true;
  }

  servoS4.write(posS4);
  if (play && waitS4)
  {
    delay(1000);
  }

  //Serial.println(posS4);

  servoS5.write(posS5);

  if ((lastS1 != posS1) | (lastS2 != posS2) | (lastS3 != posS3) | (lastS4 != posS4))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lastS1 != posS1)
        {
          buffAdd[buffPos] = posS1;
          buffPos++;
        }

        if (lastS2 != posS2)
        {
          buffAdd[buffPos] = posS2 | 0x1000;
          buffPos++;
        }

        if (lastS3 != posS3)
        {
          buffAdd[buffPos] = posS3 | 0x2000;
          buffPos++;
        }

        if (lastS4 != posS4)
        {
          buffAdd[buffPos] = posS4 | 0x3000;
          buffPos++;
        }

        if (lastS5 != posS5)
        {
          buffAdd[buffPos] = posS5 | 0x4000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(posS1);
    Serial.print("\t");
    Serial.print(posS2);
    Serial.print("\t");
    Serial.print(posS3);
    Serial.print("\t");
    Serial.print(posS4);
    Serial.print("\t");
    Serial.print(posS5);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lastS1 = posS1;
  lastS2 = posS2;
  lastS3 = posS3;
  lastS4 = posS4;
  lastS5 = posS5;

  if ( repeatPlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  }

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x7000;
    bool endOfData = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angles1 = servoS1.read();
  int angles2 = servoS2.read();
  int angles3 = servoS3.read();
  int angles4 = servoS4.read();
  int angles5 = servoS5.read();

  Serial.print(angles1);
  Serial.print("\t");
  Serial.print(angles2);
  Serial.print("\t");
  Serial.print(angles3);
  Serial.print("\t");
  Serial.print(angles4);
  Serial.println("\t");
  Serial.print(angles5);
  Serial.println("\t");

  posS1 = startS1;
  posS2 = startS2;
  posS3 = startS3;
  posS4 = startS4;
  posS5 = startS5;
  

  servoS1.write(posS1);
  servoS2.write(posS2);
  servoS3.write(posS3);
  servoS4.write(posS4);
  servoS5.write(posS5);
}