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

#include <Arduino.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#define CIRCULAR_BUFFER_XS
#include <CircularBuffer.hpp>
#include "sequence.hpp"

const int i2c_addr = 0x27;
const int lcd_columns = 16;
const int lcd_lines = 2;
LiquidCrystal_I2C lcd(i2c_addr, lcd_columns, lcd_lines);
CircularBuffer<char, lcd_columns - 2> recentDigits;

const int numServos = 5;
const int servoInterval = 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[servoInterval] = {servo1, servo2, servo3, servo4, servo5};
int angles[servoInterval] = {0, 0, 0, 0, 0};

const int baseAngle = 90;
const int pressAngle = 30;

// potentiometer readings
const int anglePot = A0;
int pressAngle();
int pressInterval();

unsigned long previousPress = 0;
const int pressInterval = 100;
const int repeatPressInterval = 150;

char currentChar;

void resetAngles();
void setAngles(char ch);
void writeAngles();
void updateScreen();

void setup()
{
  Serial.begin(9600);
  Serial.println("Starting...");
  startMillis = millis();

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

  resetAngles();
  writeAngles();

  while (recentDigits.available() > 0)
  {
    recentDigits.push(' ');
  }

  lcd.init();
  lcd.backlight();
  updateScreen();
}

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

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

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

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

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

  recentDigits.push(currentChar);

  resetAngles();
  writeAngles();

  if (repeat)
  {
    delay(repeatPressInterval);
  }
  setAngles(currentChar);
  writeAngles();

  updateScreen();

  int processedCodes = getProcessedChars() - 3;
  float speed = (float)(processedCodes) / ((float)(currentMillis - startMillis) / 1000.0);
  Serial.print("Speed: ");
  Serial.print(speed);
  Serial.print(" c/s, ");
  Serial.println();
}

void updateScreen()
{
  // Recent digits
  lcd.setCursor(0, 1);
  for (int i = 0; i < recentDigits.size(); i++)
  {
    if (i == recentDigits.size() - 4)
    {
      lcd.write(162);
    }
    lcd.print(recentDigits[i]);
  }
  lcd.write(163);

  // Progress
  lcd.setCursor(0, 0);
  lcd.print("[");
  int filled = (int)(getProgress() * (lcd_columns - 2));
  for (int i = 0; i < lcd_columns - 2; i++)
  {
    if (i < filled)
    {
      lcd.write(255);
    }
    else
    {
      lcd.write('-');
    }
  }
  lcd.print("]");
}

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

void setAngles(char 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 < numServos; i++)
  {
    servos[i].write(angles[i]);
  }
}