#include <Arduino.h>

/*
 * Author: Automatic Addison
 * Website: https://automaticaddison.com
 * Description: Calculate the angular velocity in radians/second of a DC motor
 * with a built-in encoder (forward = positive; reverse = negative) 
 */
 
// Motor encoder output pulses per 360 degree revolution (measured manually)
#define ENC_COUNT_REV (500 * 11) //12rmp motor
 
// Encoder output to Arduino Interrupt pin. Tracks the pulse count.
#define ENC_IN_RIGHT_A 10
 
// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_RIGHT_B 9

#define MOTOR_IN1      6
#define MOTOR_IN2      7

#define SPEED 92

#define POWERMETER     4
 
// True = Forward; False = Reverse
bool Direction_right = true;
 
// Keep track of the number of right wheel pulses
volatile long right_wheel_pulse_count = 0;
volatile long pulscountswap = 0;
volatile long absPulseCount = 0;
 
// One-second interval for measurements
int interval = 1000;
int swapdelay = 500;
  
// Counters for milliseconds during interval
long previousMillis = 0;
long currentMillis = 0;
 
// Variable for RPM measuerment
float rpm_right = 0;
 
// Variable for angular velocity measurement
float ang_velocity_right = 0;
float ang_velocity_right_deg = 0;
 
const float rpm_to_radians = 0.10471975512;
const float rad_to_deg = 57.29578;
bool dir = false;
bool motorruns = false;
 
void right_wheel_pulse();


// ***** function calls ******
float getVPP()
{
  float result;
  int readValue;                // value read from the sensor
  int maxValue = 0;             // store max value here
  int minValue = 4096;          // store min value here ESP32 ADC resolution
  
   uint32_t start_time = millis();
   while((millis()-start_time) < 1000) //sample for 1 Sec
   {
       readValue = analogRead(POWERMETER);
       // see if you have a new maxValue
       if (readValue > maxValue) 
       {
           /*record the maximum sensor value*/
           maxValue = readValue;
       }
       if (readValue < minValue) 
       {
           /*record the minimum sensor value*/
           minValue = readValue;
       }
   }
   
   // Subtract min from max
   result = ((maxValue - minValue) * 3.3)/4096.0; //ESP32 ADC resolution 4096
  log_i("ADC: %0.0f", result);
   return result;
 }

void run_stop()
{
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_IN1, 0);
  analogWrite(MOTOR_IN2, 0);
  motorruns = false;
  log_i("stop");

}

void run_to()
{

}

void run_left(int speed = SPEED)
{
  // run_stop();
  // delay(swapdelay);
  digitalWrite(MOTOR_IN1, LOW);
  analogWrite(MOTOR_IN2, speed);
  log_i("left");
  motorruns = true;
  dir = true;
}

void run_right(int speed = SPEED)
{
  // run_stop();
  // delay(swapdelay);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_IN1, speed);
  log_i("right");
  motorruns = true;
  dir = false;
}

bool run_to_pos()
{
  return false;
}

void swap_dir()
{
  if(dir)
  {
    run_right();
  }
  else{
    run_left();
  }
  log_i("swap");
}

long previouspulses = 0;

void calibratemotor()
{
  delay(300);
  
  bool stop = false;
  run_right();
  previouspulses = absPulseCount;
  while(!stop)
  {
    delay(100);
    if(absPulseCount - previouspulses > 40)
    {
      log_i("absPulseCount (%i)- previouspulses(%i) = %i", absPulseCount, previouspulses, absPulseCount - previouspulses);
      previouspulses = absPulseCount;
      delay(50);
    }
    else
    {
      stop = true;
      run_stop();
      log_i("left done, %i", absPulseCount);
      absPulseCount = 0;
    }
  }
  stop = false;
  delay(1000);
  run_left();
  previouspulses = absPulseCount;
  while(!stop)
  {
    delay(100);
    if(absPulseCount - previouspulses < -30)
    {
      log_i("absPulseCount (%i)- previouspulses(%i) = %i", absPulseCount, previouspulses, absPulseCount - previouspulses);
      previouspulses = absPulseCount;
      delay(50);
    }
    else
    {
      stop = true;
      run_stop();
      log_i("right done, %i", absPulseCount);
    }
  }
}

void setup() {
 
  // Open the serial port at 9600 bps
  Serial.begin(115200); 
  delay(5000);
  log_i("start sketch");
  // Set pin states of the encoder
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  pinMode(POWERMETER, ANALOG);

  
 
  // Every time the pin goes high, this is a pulse
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_pulse, RISING);
  
  log_i("init done");  
  calibratemotor();
  //run_left();
}


 
long swaptime = 0;

void loop() {
 
  // Record the time
  currentMillis = millis();
 
  // If one second has passed, print the number of pulses
  if ((currentMillis - previousMillis > interval) && (motorruns = true)) 
  {
 
    previousMillis = currentMillis;
 
    // Calculate revolutions per minute
    rpm_right = (float)(right_wheel_pulse_count * 60 / ENC_COUNT_REV);
    ang_velocity_right = rpm_right * rpm_to_radians;   
    ang_velocity_right_deg = ang_velocity_right * rad_to_deg;
     
    log_i("Pulses/s: %i", right_wheel_pulse_count);
    log_i("Speed: %0.0f", rpm_right);
    log_i("RPM");
    log_i("Angular Velocity: %0.00f", rpm_right);
    log_i("rad per second \t %f deg per second", ang_velocity_right_deg);
    log_i("powermeter %0.0f", getVPP());
    log_i("absulote pulsecount = %i", absPulseCount);

    right_wheel_pulse_count = 0;

  }

  // if(pulscountswap > ENC_COUNT_REV/4)
  // {
  //   swap_dir();
  //   pulscountswap = 0;
  // }

  // if(millis() - swaptime > 5000)
  // {
  //   swaptime = millis();
  //   swap_dir();

  // }

  // if(rpm_right < 1 && dir)
  // {
  //   run_stop();
  // }
}
 
// Increment the number of pulses by 1
void right_wheel_pulse() {
   
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);
 
  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
   
  if (Direction_right) {
    right_wheel_pulse_count++;
    pulscountswap ++;
    absPulseCount ++;
  }
  else {
    right_wheel_pulse_count--;
    pulscountswap ++;
    absPulseCount --;
  }
}

