#include <Arduino.h>
#include <DMXSerial.h>
#include <PID_v1.h>
#include <../lib/PWM/PWM.h>

// Motor Driver: IBT-2 (BTS7960)
#define LPWM_PIN 9                  // LPWM for CCW motion (postive)
#define RPWM_PIN 10                  // RPWM for CW motion (negative)
#define L_EN_PIN 11                  // Both enable pins must be high to drive motor
#define R_EN_PIN 12                  // Both enable pins must be high to drive motor

// Encoder p/n: e38s6g5-600b-g24n
#define ENCODER_A 2
#define ENCODER_B 3

// LED indicators to follow motor pwm output
#define LLED_PIN  5
#define RLED_PIN  6

#define ENCODER_PPR   600              // pulses per revolution
#define ENCODER_RATIO 2.0f              // number of times encoder spins per revolution of wall
#define MOTOR_RATIO   10.0f              // number of time motor spins per revolution of wall
#define MOTOR_MAX_RPM 330.0f            // p/n MY1016Z3 rated 300 rpm @ 24VDC; enter measured value
#define MOTOR_MIN_PWM 20                // enter value required for motor to start turning
#define MAX_MOTOR_ACCELERATION (MOTOR_MAX_RPM / 5.0f) // (RPM/s): 0 to max in 0.5s
#define MAX_WALL_ACCELERATION  MAX_MOTOR_ACCELERATION / MOTOR_RATIO
#define SPEEDRAMP_DISTANCE 0.3f      // range target velocity should be proportional to error (10 degrees = 0.0278 rev)
#define WALL_POSITION_TOLERANCE 0.0056f // acceptable error in wall position (2 degrees =  0.0056 rev)

#define SAMPLE_RATE 10000               // min time (us) between velocity calculations
#define LOOP_RATE   10                  // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE (converted to us) **

#define DMX_CHANNEL 2 // DMX Channel to read

// wall state variables:
long encoderPosition = 0;        // raw count from encoder
// double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO;
double currentPosition = 0;      // wall (revolutions)
double prevPosition = 0;         // wall (revolutions)
double targetPosition = 0;       // wall (revolutions)
double currentVelocity = 0;      // wall velocity (rpm)

// motor control variables:
double currentMotorVelocity = 0; // motor velocity (rpm)
double targetMotorVelocity = 0;  // motor velocity (rpm)

unsigned long prevSample = 0;    // time tracker for sensor sampling rate (us)
unsigned long prevControl = 0;   // time tracker for control loop (ms)
double remainingDistance = 0;    // wall distance from current to target (revolutions)
double remainingTime = 0;        // estimate of time to get from current position to target (s)
double stopTime = 0;             // estimate of time to decelerate to zero
double motorPWM = 0;             // motor PWM command level (-255/+255)
double commandVelocity = 0;      // PID commanded motor velocity (RPM)

//debug variables:
// int prevPWM = 0;                 // motor command from previous loop
// int newPWM = 0;                  // updated motor command
// int dummyTarget = 0;
// long lastToggle = 0;

// Serial Input processor:
const byte numChars = 32;
char receivedChars[numChars];   // an array to store the received data
boolean newData = false;

// Function Prototypes:
void   updateEncoder();         // react to encoder pulse and update encoder count
double encoderToPosition(long); // convert encoder count to wall position
int    Velocity_to_PWM(double); // function to convert motor velocity to PWM drive signal
// bool   recvWithEndMarker();     // read Serial input
// float  readNewTarget();          // convert Serial input to new target position
int    sign(float);             // get sign of a value: (returns +1 or -1)

//PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, ControllerDirection)
PID pid = PID(&currentMotorVelocity, &commandVelocity, &targetMotorVelocity, 255.0f/330.0f, 0.0, 0.0, DIRECT);
// PID SetSampleTime((int) LOOP_RATE);

void setup() {
  // Serial.begin(115200);

  // Configure DMX shield (MAX485 chipset)
  DMXSerial.init(DMXReceiver);
  DMXSerial.write(DMX_CHANNEL, 0);

  // Initialize GPIO pins:
  pinMode(RPWM_PIN, OUTPUT);
  pinMode(LPWM_PIN, OUTPUT);
  pinMode(L_EN_PIN, OUTPUT);
  pinMode(R_EN_PIN, OUTPUT);
  pinMode(LLED_PIN, OUTPUT);
  pinMode(RLED_PIN, OUTPUT);
  pinMode(ENCODER_A, INPUT_PULLUP);
  pinMode(ENCODER_B, INPUT_PULLUP);

  // Start encoder:
  attachInterrupt(digitalPinToInterrupt(ENCODER_A), updateEncoder, RISING);

  // Start the Motor Driver - Set Frequency, start both PWM low, Enable both sides:
  InitTimersSafe();
  SetPinFrequencySafe(9, 10000);
  SetPinFrequencySafe(10, 10000);
  digitalWrite(LPWM_PIN, LOW);
  digitalWrite(RPWM_PIN, LOW);
  digitalWrite(L_EN_PIN, HIGH);
  digitalWrite(R_EN_PIN, HIGH);

  pid.SetSampleTime(LOOP_RATE);
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-330, 330);

  // initialize loop timers
  prevSample  = millis();
  prevControl = millis();
}

void loop() {

  if (DMXSerial.noDataSince() < 5000) {
    targetPosition = (float)(DMXSerial.read(DMX_CHANNEL) - 101) / 100.0f; // 0 is disabled. position values are 1-201, 101 is center, <101 is CW, >101 is CCW. 1 & 201 are 360 degrees
    digitalWrite(R_EN_PIN, HIGH);
    digitalWrite(L_EN_PIN, HIGH);

    if (micros() < prevSample) {
      // micros timer rolled over, so reset sample timer:
      // Serial.println("micros() roll-over");
      prevSample = micros();
      currentPosition = encoderToPosition(encoderPosition);
    } else if (micros() - prevSample >= SAMPLE_RATE) {
      // this can happen faster than control loop so that (TBD) smoothing function can be added
      prevPosition = currentPosition;
      double dt = (float (micros() - prevSample)) / (1.0e6f);           // time since last sample (s)
      currentPosition = encoderToPosition(encoderPosition);             // get wall position (rev) from encoder reading
      currentVelocity = 60.0f * (currentPosition - prevPosition) / dt;  // wall speed (rpm)    
      currentMotorVelocity = currentVelocity * MOTOR_RATIO;             // motor speed (rpm), assuming no slipping
      remainingDistance = targetPosition - currentPosition;             // position error (rev)
      if (currentVelocity == 0) {
        remainingTime = 1e6;
      } else {
        remainingTime = remainingDistance/currentVelocity;
      }

      if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) {
        // we are close enough, stop moving:
        targetMotorVelocity = 0.0f;
        remainingTime = 0.0f;
      } else {
        targetMotorVelocity =  sign(remainingDistance) * min(abs(remainingDistance) / SPEEDRAMP_DISTANCE * MOTOR_MAX_RPM, MOTOR_MAX_RPM);
      }

      // prevPWM = motorPWM;
      prevSample = micros();

      /*
      Serial.print("sample updated:");
      Serial.print(" pos: ");Serial.print(currentPosition);
      Serial.print(" vel: ");Serial.print(currentVelocity);
      Serial.print(" error: ");Serial.print(remainingDistance);
      Serial.println();
      */
    }

      if (pid.Compute()) {
        // newPWM = motorPWM;

        motorPWM = Velocity_to_PWM(commandVelocity);

        // update motor controller command:
        if (abs(motorPWM) < MOTOR_MIN_PWM) {
          // Serial.println("PWM < MIN");
          digitalWrite(RPWM_PIN, LOW);
          digitalWrite(RLED_PIN, LOW);
          digitalWrite(LPWM_PIN, LOW);
          digitalWrite(LLED_PIN, LOW);
        } else if (motorPWM > 0) {
          // Serial.println("PWM > 0");
          digitalWrite(RPWM_PIN, LOW);
          digitalWrite(RLED_PIN, LOW);
          pwmWrite(LPWM_PIN, motorPWM);
          pwmWrite(LLED_PIN, motorPWM);
        } else {     
          // Serial.println("Else");
          digitalWrite(LPWM_PIN, LOW);
          digitalWrite(LLED_PIN, LOW);
          pwmWrite(RPWM_PIN, -motorPWM);
          pwmWrite(RLED_PIN, -motorPWM);
        }

        // generate some debug output:
        // Serial.print(">time: ");
        // Serial.print(millis());
        // Serial.print(", ePos: ");
        // Serial.print(encoderPosition);
        // Serial.print(", wPos: ");
        // Serial.print(currentPosition);
        // Serial.print(", tPos: ");
        // Serial.print(targetPosition);
        // Serial.print(", wVel: ");
        // Serial.print(currentVelocity);
        // Serial.print(", rDist: ");
        // Serial.print(remainingDistance);
        // //Serial.print(", rTime: ");
        // //Serial.print(remainingTime);
        // Serial.print(", sTime:");
        // Serial.print(stopTime);
        // Serial.print(", tVel: ");
        // Serial.print(targetMotorVelocity);
        // Serial.print(", mPWM: ");
        // Serial.println(motorPWM);
    }
    // make sure DMX is still alive:  
  } else {
    digitalWrite(R_EN_PIN, LOW);
    digitalWrite(L_EN_PIN, LOW);
  }

    // react to Serial input (new target position):
    // if (recvWithEndMarker()) {targetPosition = readNewTarget();}
}

// return +1 if positive or -1 if negative
int sign(float val) {
  // return +1 if positive or -1 if negative
  if (val >= 0) {return 1;} else {return -1;}
}

// got an interrupt on ENCODER_A, so update encoder position
void updateEncoder() {
  if (digitalRead(ENCODER_B) == LOW) {
    encoderPosition--; // CCW
  } else {
    encoderPosition++; // CW
  }
}

double encoderToPosition(long encoderPosition) {
  return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO;
}

int Velocity_to_PWM(double commandVelocity) {
  if (abs(commandVelocity) > 1) {
    return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(commandVelocity)), 255) * sign(commandVelocity);
  }
  return 0;
}

// bool recvWithEndMarker() {
//     static byte ndx = 0;
//     char endMarker = '\n';
//     char rc;
    
//     while (Serial.available() > 0 && newData == false) {
//         rc = Serial.read();

//         if (rc != endMarker) {
//             receivedChars[ndx] = rc;
//             ndx++;
//             if (ndx >= numChars) {
//                 ndx = numChars - 1;
//             }
//         }
//         else {
//             receivedChars[ndx] = '\0'; // terminate the string
//             ndx = 0;
//             newData = true;
//         }
//     }
//     return newData;
// }

// float readNewTarget() {
//     Serial.print("New Target Position: ");
//     Serial.println(receivedChars);
//     delay(500);
//     newData = false;
//     return atof(receivedChars);
// }