// Based on Servo Showcase
// Version 1, July 25, 2021 by Koepel, Public Domain.
// Version 2, October 7, 2023 by Koepel, Public Domain.

#include <Servo.h>
#include "sequence.c"

#define NUM_SERVOS 5
#define INTERVAL 20 // 10 or 20 ms are common intervals between instructions

unsigned long previousMillis;
unsigned long startMillis;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

Servo servos[NUM_SERVOS] = { servo1, servo2, servo3, servo4, servo5 };
int angles[NUM_SERVOS] = { 0, 0, 0, 0, 0 };

const int baseAngle = 90;
const int pressAngle = 60;

unsigned long previousPress = 0;
const int pressInterval = 500;
const int movementDuration = pressInterval / 3;

char currentChar;
const int sequenceLength = sizeof(sequence);

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

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

  resetAngles();
  writeAngles();
}

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

  if (currentMillis - previousMillis < INTERVAL) {
    return;
  }
  previousMillis = currentMillis;

  if ( currentMillis - previousPress >= pressInterval) {
    return;
  }
  previousPress = currentMillis;

  if (!isNextChar()) {
    Serial.println("");
    Serial.println("Done! Exiting...");
    resetAngles();
    writeAngles();
    delay(movementDuration);
    exit(0);
  }

  char nextChar = getNextChar();
  bool repeat = currentChar == nextChar;
  currentChar = nextChar;

  resetAngles();
  writeAngles();

  delay(movementDuration);
  setAngles(currentChar);
  writeAngles();
  delay(movementDuration);

  resetAngles();
  writeAngles();

  /*if (repeat) {
    delay(pressInterval / 2);
    resetAngles();
    writeAngles();
  }*/  
}

int charIndex = 0;
char getNextChar() {
    char nextChar = sequence[charIndex];
    charIndex++;
    return nextChar;
}

bool isNextChar() {
  return charIndex < sequenceLength - 1;
}

void resetAngles() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    angles[i] = baseAngle;
  }
}

void setAngles(char ch) {
  Serial.print(ch);

  if (!isDigit(ch)) {
    Serial.print("Not a digit: ");
    Serial.println(ch);
    return;
  }

  resetAngles();
  switch (ch) {
    case '1':
      angles[0] = baseAngle + pressAngle;
      break;
    case '2':
      angles[0] = baseAngle - pressAngle;
      break;
    case '3':
      angles[1] = baseAngle + pressAngle;
      break;
    case '4':
      angles[1] = baseAngle - pressAngle;
      break;
    case '5':
      angles[2] = baseAngle + pressAngle;
      break;
    case '6':
      angles[2] = baseAngle - pressAngle;
      break;
    case '7':
      angles[3] = baseAngle + pressAngle;
      break;
    case '8':
      angles[3] = baseAngle - pressAngle;
      break;
    case '9':
      angles[4] = baseAngle + pressAngle;
      break;
    case '0':
      angles[4] = baseAngle - pressAngle;
      break;
  }
}

void writeAngles() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    servos[i].write(angles[i]);
  }
}