#include <Servo.h>

constexpr uint8_t POS_MAX = 500;
constexpr uint8_t REC_BUTTON = 4;
constexpr uint8_t ERASE_BUTTON = 5;
constexpr uint8_t PLAY_BUTTON = 3;
constexpr uint8_t SAVE_BUTTON = 2;
constexpr uint8_t RST_BUTTON = 6;
constexpr int SAVE_INTERVAL = 2000;

uint8_t progState;
enum progStates { waiting, recording, playing };

Servo servos[4];
const int NUM_SERVOS = 4;

bool recBtnState;
bool eraseBtnState;
bool saveBtnState;
bool playBtnState;
bool rstBtnState;

int playIndex;
int recordIndex;
int srvoPos[4];
uint8_t srvoSpd = 20;
uint8_t playBackSpeed = 1;
uint8_t positions[5][POS_MAX];

unsigned long buttonInterval;
unsigned long recordInterval;
unsigned long playBackInterval;
unsigned long joyStickInterval;


bool playBackComplete()
{
  unsigned long now = micros();
  if (now - playBackInterval > playBackSpeed)
  {
    bool done = true;
    for (int i = 0; i < NUM_SERVOS; ++i)
    {
      uint8_t next = positions[i][playIndex];

      if (srvoPos[i] != next)
      {
        srvoPos[i] = srvoPos[i] < next ? srvoPos[i] + 1 : srvoPos[i] - 1;
        servos[i].write(srvoPos[i]);
        done = false;
      }
      else
      {
        Serial.print(F("Servo "));
        Serial.print(i);
        Serial.print(F(" moved to: "));
        Serial.println(positions[i][playIndex]);
      }
    }
    playIndex = done ? playIndex + 1 : playIndex;
    playBackInterval = now;
  }

  return playIndex == recordIndex;
}


void savePosition()
{
  if (recordIndex < POS_MAX)
  {
    for (int i = 0; i < NUM_SERVOS; ++i)
    {
      Serial.print(F("Servo "));
      Serial.print(i);
      Serial.print(F(" pos saved: "));
      Serial.println(srvoPos[i]);
      positions[i][recordIndex] = srvoPos[i];
    }
    recordIndex++;
  }
  else
  {
    Serial.println(F("Maximum positions reached."));
  }
}

void record(unsigned long now)
{
  if ((now - recordInterval)  > SAVE_INTERVAL)
  {
    savePosition();
    recordInterval = now;
  }
}

void readJoyStick(unsigned long now)
{
  if (now - joyStickInterval > srvoSpd)
  {
    for (int i = 0; i < NUM_SERVOS; ++i)
    {
      int joystickPosition = analogRead(i);

      if (joystickPosition > 524 && srvoPos[i] < 180)
      {
        srvoPos[i]++;
        servos[i].write(srvoPos[i]);
      }

      if (joystickPosition < 500 && srvoPos[i] > 0)
      {
        srvoPos[i]--;
        servos[i].write(srvoPos[i]);
      }

      joyStickInterval = now;
    }
  }
}

void readRecordButton(unsigned long now)
{
  if (now - buttonInterval > 250)
  {
    uint8_t state = digitalRead(REC_BUTTON);

    if (state == LOW && state != recBtnState)
    {
      progState = progState == recording ? waiting : recording;
      const char* state = progState == recording ? "started." : "stopped.";
      Serial.print(F("Recording "));
      Serial.println(state);
      buttonInterval = now;
    }

    recBtnState = state;
  }
}

void readSaveButton(unsigned long now)
{
  if (now - buttonInterval > 250)
  {
    uint8_t state = digitalRead(SAVE_BUTTON);

    if (state == LOW && state != saveBtnState)
    {
      savePosition();
      buttonInterval = now;
    }

    saveBtnState = state;
  }
}

void readPlayButton(unsigned long now)
{
  if (now - buttonInterval > 250)
  {
    uint8_t state = digitalRead(PLAY_BUTTON);

    if (state == LOW && state != playBtnState)
    {
      progState = progState == playing ? waiting : playing;
      const char* state = progState == playing ? "started." : "stopped.";
      Serial.print(F("Playback "));
      Serial.println(state);
      buttonInterval = now;
      playIndex = 0;
    }

    playBtnState = state;
  }
}

void readEraseButton(unsigned long now)
{
  if (now - buttonInterval > 250)
  {
    uint8_t state = digitalRead(ERASE_BUTTON);

    if (state == LOW && state != eraseBtnState)
    {
      for (int i = 0; i < 5; ++i)
      {
        memset(&positions[i], 0, sizeof(positions[i]));
      }
      Serial.println(F("Recording erased."));
      buttonInterval = now;
      recordIndex = 0;
    }

    eraseBtnState = state;
  }
}

void readResetButton(unsigned long now)
{
  if (now - buttonInterval > 250)
  {
    uint8_t state = digitalRead(RST_BUTTON);

    if (state == LOW && state != rstBtnState)
    {
      servos[0].write(90);
      servos[1].write(90);

      Serial.println(F("Reset button pressed."));
      buttonInterval = now;
    }

    rstBtnState = state;
  }
}

void setup()
{
  pinMode(REC_BUTTON, INPUT_PULLUP);
  pinMode(RST_BUTTON, INPUT_PULLUP);
  pinMode(SAVE_BUTTON, INPUT_PULLUP);
  pinMode(PLAY_BUTTON, INPUT_PULLUP);
  Serial.begin(9600);
  for (int i = 0; i < 4; ++i)
  {
    srvoPos[i] = 90;
    servos[i].attach(i + 9);
    servos[i].write(srvoPos[0] = 90);
  }
}

void getInput(unsigned long now)
{
  readJoyStick(now);
  readPlayButton(now);
  readSaveButton(now);
  readEraseButton(now);
  readRecordButton(now);
  readResetButton(now);
}

void playBack(unsigned long now)
{
  readPlayButton(now);

  if (playBackComplete())
  {
    Serial.println(F("Playback complete."));
    progState = waiting;
  }
}


void loop()
{
  unsigned long now = millis();

  switch (progState)
  {
    case playing:
      playBack(now);
      break;
    case waiting:
      getInput(now);
      break;
    case recording:
      getInput(now);
      record(now);
      break;
  }
}