// https://forum.arduino.cc/t/problem-with-setpwm-and-loop/1121851

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define PRESSED   LOW
#define RELEASED HIGH

#define   servopalac_num 2
#define    servokazi_num 3
#define servosrednji_num 4
#define  servodomali_num 5
#define    servomali_num 6
#define   servozglob_num 7
#define servobiceps1_num 8
#define servobiceps2_num 9

const int   STOP_BUTTON_PIN =  2;
const int RESUME_BUTTON_PIN =  3;
const int TOUCH_SENSOR3_PIN =  4;
const int TOUCH_SENSOR4_PIN =  5;
const int TOUCH_SENSOR5_PIN =  6;
const int           LED_PIN = 13;


struct ServoStep {
  uint8_t RelativeDelay;
  uint8_t Servo;
  uint16_t position;
};

ServoStep Dance[] = {
  { 10,   servopalac_num, 615 }, 
  {  0,    servokazi_num, 615 }, 
  {  0, servosrednji_num, 615 }, 
  {  0,  servodomali_num, 615 }, 
  {  0,    servomali_num, 615 }, 
  {  0, servobiceps1_num, 225 }, 
  {  0, servobiceps2_num, 410 }, 
  { 30, servobiceps1_num, 300 }, 
  {  0,   servopalac_num, 205 }, 
  {  0,    servokazi_num, 205 }, 
  {  0, servosrednji_num, 205 }, 
  {  0,  servodomali_num, 205 }, 
  { 30,    servomali_num, 205 }, 
  {  0,   servopalac_num, 410 }, 
  {  0,    servokazi_num, 410 }, 
  {  0, servosrednji_num, 410 }, 
  {  0,  servodomali_num, 410 }, 
  { 30,    servomali_num, 410 }, 
  { 10, servobiceps1_num, 350 }, 
  { 10, servobiceps1_num, 270 }, 
  { 10, servobiceps1_num, 350 }, 
  { 10, servobiceps1_num, 270 }, 
  { 30, servobiceps1_num, 350 }, 
  {  0,   servopalac_num, 205 }, 
  {  0,    servokazi_num, 205 }, 
  {  0, servosrednji_num, 205 }, 
  {  0,  servodomali_num, 205 }, 
  { 30,    servomali_num, 205 }, 
  { 30, servobiceps2_num, 505 }, 
  {  0,   servopalac_num, 505 }, 
  {  0,    servokazi_num, 505 }, 
  {  0, servosrednji_num, 550 }, 
  {  0,  servodomali_num, 505 }, 
  { 30,    servomali_num, 600 }, 
  { 30, servobiceps1_num, 300 }, 
  { 30, servobiceps2_num, 250 }, 
  { 50,   servozglob_num, 205 }, 
  { 30,   servozglob_num, 505 }, 
  { 30, servobiceps2_num, 505 }, 
  { 30, servobiceps1_num, 270 }, 
  { 30, servobiceps1_num, 350 }, 
  {  0,   servopalac_num, 205 }, 
  {  0,    servokazi_num, 205 }, 
  {  0, servosrednji_num, 205 }, 
  {  0,  servodomali_num, 205 }, 
  { 30,    servomali_num, 205 }, 
  { 30, servobiceps2_num, 410 }, 
  { 30, servobiceps2_num, 225 }, 
  {  0,   servopalac_num, 615 }, 
  {  0,    servokazi_num, 615 }, 
  {  0, servosrednji_num, 615 }, 
  {  0,  servodomali_num, 615 }, 
  { 30,    servomali_num, 615 }
};

// bool should_stop = false;
bool     runFlag;
uint64_t timeReference;
uint8_t  stepIndex;


void setup() {
  Serial.begin(115200);
  pwm.begin();
  pwm.setPWMFreq(60);
  pinMode(STOP_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RESUME_BUTTON_PIN, INPUT_PULLUP);
  pinMode(TOUCH_SENSOR3_PIN, INPUT);
  pinMode(TOUCH_SENSOR4_PIN, INPUT);
  pinMode(TOUCH_SENSOR3_PIN, INPUT);
  stepIndex = 0;
  runFlag = true;
  Serial.println("Servo Arm Ready");
  Serial.print("sizeof(Dance) ");
  Serial.println(sizeof(Dance)/sizeof(ServoStep));
  timeReference = millis();
}

void loop() {
  if(runFlag && digitalRead(STOP_BUTTON_PIN) == PRESSED) {
    runFlag = false;
    Serial.print("Process paused at Index ");
    Serial.println(stepIndex);
    pwm.sleep();
  }

  if(!runFlag && digitalRead(RESUME_BUTTON_PIN) == PRESSED) {
    uint8_t localStepIndex;
    runFlag = true;
    Serial.println("Process Resumed");
    pwm.wakeup();
    timeReference = millis();
  }

  if(runFlag && millis() > timeReference + 100 * Dance[stepIndex].RelativeDelay) { // is step delay due ?
    timeReference += 100 * Dance[stepIndex].RelativeDelay; // update time reference, a += 3 means a = a + 3
    pwm.setPWM(Dance[stepIndex].Servo, 0, Dance[stepIndex].position);
    Serial.print("stepIndex ");
    Serial.print(stepIndex);
    Serial.print(", Servo ");
    Serial.print(Dance[stepIndex].Servo);
    Serial.print(" to position ");
    Serial.print(Dance[stepIndex].position);
    Serial.print("  waiting ");
    Serial.println(Dance[stepIndex].RelativeDelay);
    if(stepIndex < sizeof(Dance) / sizeof(ServoStep)) {
      stepIndex++;
    }
    else {
      stepIndex = 0;
    }
  }
}