#include <Arduino.h>
//#include <DMXSerial.h>
#include <PID_v1.h>
#include <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   6000             // pulses per revolution
#define ENCODER_RATIO 1.0f            // number of times encoder spins per revolution of wall
#define MOTOR_RATIO   5.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 30              // 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 WALL_POSITION_TOLERANCE 0.00056 // acceptable error in wall position (2 degrees =  0.0056 rev)

#define SAMPLE_RATE 100.0f                // min time (ms) between velocity calculations
#define LOOP_RATE   100.0f                // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE **

const int DMX_CHANNEL = 1;

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)
double targetMotorVelocity = 0; // motor velocity (rpm)

unsigned long prevSample = 0;   // time tracker for sensor sampling rate
unsigned long prevControl = 0;  // time tracker for control loop
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
int motorPWM = 0;               // motor PWM level to achieve target motor velocity (0-255)

// 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
void   recvWithEndMarker();     // read Serial input
void   showNewData();           // act on Serial input

PID pid = PID(&currentPosition, &targetMotorVelocity, &targetPosition, 600, 0.25, 5.0, DIRECT);

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);
  digitalWrite(LPWM_PIN, LOW);
  digitalWrite(RPWM_PIN, LOW);
  digitalWrite(L_EN_PIN, HIGH);
  digitalWrite(R_EN_PIN, HIGH);

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

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

void loop() {
  if (millis() < prevControl) {
    // millis timer rolled over, so reset sample and control timers:
    prevSample = millis();
    prevControl = millis();
    currentPosition = encoderToPosition(encoderPosition);
  }

  if (millis() - prevSample >= SAMPLE_RATE) {
    // this happens faster than control loop so that TBD smoothing function can be added
    prevPosition = currentPosition;
    currentPosition = encoderToPosition(encoderPosition);                                  // get wall position (revolutions) from encoder reading
    currentVelocity = 60000.0f * (currentPosition - prevPosition) / (float)(millis() - prevSample);  // wall speed (rpm)
    prevSample = millis();
  }

  if (millis() - prevControl >= LOOP_RATE) {
    // targetMotorVelocity = currentVelocity * MOTOR_RATIO;        // begin with assumption we are going the right speed
    double remainingDistance = targetPosition - currentPosition;
    // double remainingTime; // projected running time assuming velocity doesn't change

    // if (currentVelocity == 0) {
    //   remainingTime = 1e6;
    // } else {
    //   remainingTime = remainingDistance / currentVelocity;
    // }
    // if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) {
    //   // we are close enough, stop moving:
    //   if (abs(targetMotorVelocity) < MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f) {
    //     // we aren't going very fast so just command zero:
    //     targetMotorVelocity = 0;
    //   } else {
    //     // correct by max allowable rate:
    //     targetMotorVelocity =- (abs(targetMotorVelocity) / targetMotorVelocity) * MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
    //   }
    // } else if (remainingTime < 0) {
    //   // moving in the wrong direction, so course correct:
    //   if (remainingDistance > 0) {
    //     // need to go CCW, so target velocity is more positive than current velocity
    //     // but if we are close enough, don't over accelerate; just calculate target speed
    //     targetMotorVelocity = min(targetMotorVelocity + MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f, (0.5 * remainingDistance / ((float)LOOP_RATE/1000.0f)) * MOTOR_RATIO);
    //   } else {
    //     // need to go CW, so target velocity is more negative than current velocity
    //     targetMotorVelocity = max(targetMotorVelocity - MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f, (0.5 * remainingDistance / ((float)LOOP_RATE/1000.0f)) * MOTOR_RATIO);
    //   }
    // } else {
    //   // moving in the correct direction, so check if we should start slowing down:
    //   stopTime = abs(currentVelocity) / MAX_WALL_ACCELERATION;
    //   if (stopTime > (remainingTime - 2*(float)LOOP_RATE/1000.0f)) {
    //     // we need to be slowing down within the next two control loops, so start now:
    //     if (remainingDistance > 0) {
    //       // going CCW, so target should adjust in CW direction
    //       targetMotorVelocity = targetMotorVelocity - MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
    //       if (targetMotorVelocity < 0) {
    //         targetMotorVelocity = 0;
    //       }
    //     } else {
    //       // going CW, so target should adjust in CCW direction
    //       targetMotorVelocity = targetMotorVelocity + MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
    //       if (targetMotorVelocity > 0) {
    //         targetMotorVelocity = 0;
    //       }
    //     }
    //   } else {
    //     // we still have a ways to go, so we can accelerate:
    //     if (remainingDistance > 0) {
    //       // going CCW, so target should adjust more CCW until MAX
    //       targetMotorVelocity = min(MOTOR_MAX_RPM, targetMotorVelocity + MAX_MOTOR_ACCELERATION * LOOP_RATE/1000.0f);
    //     } else {
    //       // going CW, so target should adjust more CW until -MAX
    //       targetMotorVelocity = max(-MOTOR_MAX_RPM, targetMotorVelocity - MAX_MOTOR_ACCELERATION * LOOP_RATE/1000.0f);
    //     }
    //   }
    // }


    // int motorPWM = Velocity_to_PWM(targetMotorVelocity);
    // targetMotorVelocity += min((remainingDistance * 0.5) - targetMotorVelocity, MAX_MOTOR_ACCELERATION / LOOP_RATE / 1000.0f);

    // if (abs(remainingDistance) > WALL_POSITION_TOLERANCE) {
    //   if (((remainingDistance / 0.16 * 330) - targetMotorVelocity) > 0) {
    //     targetMotorVelocity = currentVelocity + min((remainingDistance / 0.16 * 330) - currentVelocity, MAX_MOTOR_ACCELERATION * (LOOP_RATE / 1000.0f));
    //   } else {
    //     targetMotorVelocity = currentVelocity + max((remainingDistance / 0.16 * 330) - currentVelocity, - MAX_MOTOR_ACCELERATION * (LOOP_RATE / 1000.0f));
    //   }
    //   if (targetMotorVelocity > MOTOR_MAX_RPM) {
    //     targetMotorVelocity = MOTOR_MAX_RPM;
    //   }
    //   if (targetMotorVelocity < - MOTOR_MAX_RPM) {
    //     targetMotorVelocity = - MOTOR_MAX_RPM;
    //   }
      
    // } else if (currentVelocity < 1){
    //   targetMotorVelocity = 0;
    // }

    pid.Compute();
    

    int motorPWM = Velocity_to_PWM(targetMotorVelocity);

    if (targetMotorVelocity == 0)  {
      digitalWrite(RPWM_PIN, LOW);
      digitalWrite(RLED_PIN, LOW);
      digitalWrite(LPWM_PIN, LOW);
      digitalWrite(LLED_PIN, LOW);
    } else if (targetMotorVelocity > 0) {
      digitalWrite(RPWM_PIN, LOW);
      digitalWrite(RLED_PIN, LOW);
      pwmWrite(LPWM_PIN, motorPWM);
      pwmWrite(LLED_PIN, motorPWM);
    } else {     
      digitalWrite(LPWM_PIN, LOW);
      digitalWrite(LLED_PIN, LOW);
      pwmWrite(RPWM_PIN, motorPWM);
      pwmWrite(RLED_PIN, motorPWM);
    }
    
    prevControl = millis();

    Serial.print(">ePos: ");
    Serial.print(encoderPosition);
    Serial.print(", wPos: ");
    Serial.print(currentPosition);
    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);

  }

    recvWithEndMarker();
    showNewData();

  // delay(5);
  
  // make sure DMX is still alive:  
    // // } else {
    // //   digitalWrite(R_EN_PIN, 0);
    // //   digitalWrite(L_EN_PIN, 0);
    // // }
}

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 motorVelocity) {
  if (abs(motorVelocity) > 0) {
    return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(motorVelocity)), 255);
  }
  return 0;
}

void 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;
        }
    }
}

void showNewData() {
    if (newData == true) {
        Serial.print("This just in ... ");
        Serial.println(receivedChars);
        newData = false;
    }
}