#include <Servo.h>
#include "Ramp.h"

#define ccwPBPin  2
#define cwPBPin   3
#define cwLED     4
#define ccwLED    5
#define enablePin 8

const uint8_t numberOfServo = 3;
const uint8_t servoPins[] = {11, 10, 9};
const uint8_t rampPins[] = {A0, A1, A2};


const float minTime = 0.5f;
const float maxTime = 5.0f;
const int maxdegreeAngle = 180;


Servo servo[numberOfServo];
bool stop[numberOfServo];
bool start[numberOfServo];
bool enable = false;
bool cwState = false;
bool ccwState = false;
bool directionState = false;

unsigned long startTime[numberOfServo];

int rawValue[numberOfServo];
float settingTime[numberOfServo];
float rate[numberOfServo];
float degreeAngle[numberOfServo];
float lastAngle[numberOfServo];

MicroBeaut_Ramp rampRate[numberOfServo];

void setup()
{
  Serial.begin(9600);
  // Input Mode
  pinMode(cwPBPin, INPUT);
  pinMode(ccwPBPin, INPUT);
  pinMode(enablePin, INPUT_PULLUP);

  // Output Mode
  pinMode(cwLED, OUTPUT);
  pinMode(ccwLED, OUTPUT);

  // Servo Pin
  for (uint8_t index = 0; index < numberOfServo; index++) {
    servo[index].attach(servoPins[index]);
  }
}

void loop()
{
  cwState = digitalRead(cwPBPin);   // Read CW PB State
  ccwState = digitalRead(ccwPBPin); // Read CCW PB State
  enable = digitalRead(enablePin);  // Read Enable Status

  directionState = (directionState & !ccwState) | cwState; // Selection: 0 = CCW, 1 = CW

  digitalWrite(cwLED, directionState);    // CW Status
  digitalWrite(ccwLED, !directionState);  // CCW Status

  for (uint8_t index = 0; index < numberOfServo; index++) {
    rawValue[index] = analogRead(rampPins[index]);
    settingTime[index] = Scale(rawValue[index], minTime, maxTime);
    rate[index] = (float)maxdegreeAngle / settingTime[index];
    if (enable) {
      if (directionState) {
        degreeAngle[index] = rampRate[index].rampControl(180.0f, rate[index], enable);  // CW Direction
      }
      else {
        degreeAngle[index] = rampRate[index].rampControl(0.0f, rate[index], enable);    // CCW Direction
      }
    }
    servo[index].write(degreeAngle[index]);
    stop[index] = (degreeAngle[index] == 0 && lastAngle[index] != 0);
    stop[index] |= (degreeAngle[index] == 180 && lastAngle[index] != 180);
    start[index] = lastAngle[index] == 0 && degreeAngle[index] != 0;
    start[index] |= lastAngle[index] == 180 && degreeAngle[index] != 180;
    if (start[index]) {
      startTime[index] = micros();
    }

    if (stop[index]) {
      Serial.print("Servo " + String(index + 1));
      Serial.print(directionState == true ? " CW" : " CCW");
      Serial.print(": Setting Time:" + String(settingTime[index]) + " s");
      Serial.print(": Speed:" + String(rate[index]) + " d/s Time:");
      Serial.print(String(float(micros() - startTime[index]) / 1000000.0));
      Serial.println(" s");
    }
    lastAngle[index] = degreeAngle[index];
  }
}

float Scale(int const inputValue, float const minEU, float const maxEU) {
  return ((float)inputValue / 1023.0) * (maxEU - minEU) + minEU;      // Return scaling value
}