// Includes
#include <math.h>
//#include "helpers.h"


// Pin declares
const int MOTOR_PWM_PIN = 3;      // PWM output pin for motor
const int MOTOR_DIR_PIN_1 = 4;    // Direction control pin 1
const int MOTOR_DIR_PIN_2 = 5;    // Direction control pin 2


const int pot1Pin = A0;
const int pot2Pin = A1;


float pot1Value = 0.0;
float pot2Value = 0.0;


String inputBuffer = "";
int receivedValue = 0;


static bool baselineSet = false;
static int baselineCurrent = 0;


//const int switchPin = 2;
//bool straight = true;




int customMap(int x, int in_min, int in_max, int out_min, int out_max) {
  float scale = (float)(out_max - out_min) / (float)(in_max - in_min);
  return (int)(out_min + ((x - in_min) * scale));
}


void calculate_angles(float pot1Value, float pot2Value, float &handle_angle, float &base_angle){
  // code that converts the encoder position to the proper angle values
  float mhandle = 0.243;
  float bhandle = -36.9;
  float mbase = 0.258;
  float bbase = -40;


  handle_angle = pot1Value * mhandle + bhandle;
  base_angle = pot2Value * mbase + bbase;
}


void direction_test(float handle_angle, int i){
  // Function that tests what direction the force should be implemented
  // Needs to store handle angle and velocity in arrays so that the last ten velocity values can be averaged
  // to figure out what direction the force should be implemented. The force should be implemented in the opposite direction of the velocity.
 
  static float angleHistory[11];
  static int index = 0;
  static bool filled = false;


  static int lastDirection = 0; // 0 = no force, 1 = CCW, -1 = CW


  angleHistory[index] = handle_angle;
  index = (index + 1) % 11;


  if (!filled && index == 0) filled = true;


  if (filled) {
    float velocitySum = 0.0;
    for (int i = 0; i < 10; i++) {
      int curr = (index + i) % 11;
      int next = (index + i + 1) % 11;
      velocitySum += (angleHistory[next] - angleHistory[curr]);
    }


    float averageVelocity = velocitySum / 10.0;
    int newDirection = 0;


    if (averageVelocity > 0.5) {
      newDirection = 1; // CCW force
    } else if (averageVelocity < -0.5) {
      newDirection = -1; // CW force
    }


    bool currentSafe = (i < (baselineCurrent + 30));


    // Allow initial direction set, or maintaining same direction
    // Block only if trying to change to a different direction
    if (newDirection != lastDirection && !currentSafe) {
      // Block direction change
      return;
    }


    lastDirection = newDirection;


    if (newDirection == 1) {
      digitalWrite(4, LOW);
      digitalWrite(5, HIGH);
    } else if (newDirection == -1) {
      digitalWrite(4, HIGH);
      digitalWrite(5, LOW);
    } else {
      digitalWrite(4, LOW);
      digitalWrite(5, LOW);
    }
  }
}






    // Optional: Debug print
    //Serial.print("Avg Vel: ");
    //Serial.println(averageVelocity);
 






/*
void calculate_direction(float handle_angle){


  //float handleLength = 0.05; // in meters ?
  //float scaled_value = 0.4/175.0;
  float direction;
  straight = digitalRead(switchPin) == LOW;


  //bool straight = true;


  // Keep track of straight vs turn conditions and det directions based on that


  // Switch statement that checks if straight or turn (add button debounce code to this later)


  // if straight = true && button pressed
    // straight = false
  // else if straight = false && button pressed
    // straight = true


  //if in straight
  if (straight == true){
    // if handle to the right
    if (handle_angle >= 100.0){
      // Force to center line (directie away from center line (direction = -1)
      //direction = 1.0;
      digitalWrite(MOTOR_DIR_PIN_1, LOW);
      digitalWrite(MOTOR_DIR_PIN_2, HIGH);


    } else if (handle_angle <= 80.0 ){
      digitalWrite(MOTOR_DIR_PIN_1, HIGH);
      digitalWrite(MOTOR_DIR_PIN_2, LOW);
      }else {
      digitalWrite(MOTOR_DIR_PIN_1, LOW);
      digitalWrite(MOTOR_DIR_PIN_2, LOW);
    }
  } else{
    if (handle_angle >= 100.0) {
      digitalWrite(MOTOR_DIR_PIN_1, HIGH);
      digitalWrite(MOTOR_DIR_PIN_2, LOW);
    } else if (handle_angle <= 80.0 ) {
      digitalWrite(MOTOR_DIR_PIN_1, LOW);
      digitalWrite(MOTOR_DIR_PIN_2, HIGH);
    } else{
      digitalWrite(MOTOR_DIR_PIN_1, LOW);
      digitalWrite(MOTOR_DIR_PIN_2, LOW);
    }
  }


  // Average i component for filtering (try without this first)


  //float force = i* scaled_value* direction;
  //float motor_torque = i //* direction;




  //return motor_torque;
}


*/


void setup() {


  Serial.begin(9600);  // UART
  TCCR2A = (TCCR2A & 0b11111100) | 0b01;  // WGM20 = 1, WGM21 = 0 → Phase Correct PWM
  TCCR2B = (TCCR2B & 0b11111000) | 0x01;  // Prescaler = 1  digitalWrite(MOTOR_DIR_PIN_1, HIGH);
  //digitalWrite(MOTOR_DIR_PIN_2, LOW);
  pinMode(3,OUTPUT); //pwm
  pinMode(4,OUTPUT); // dir 1
  pinMode(5,OUTPUT); // dir 2
  //pinMode(switchPin, INPUT_PULLUP);  // Set switch pin as input


}


void loop() {


  //updateSwitchState(); // check switch state
  //straight = digitalRead(switchPin) == LOW;


  //Serial.print(straight);
  //Serial.print(",");


  // === 1. Read potentiometers ===
  pot1Value = analogRead(pot1Pin);  // 0–1023
  pot2Value = analogRead(pot2Pin);
  //float updated_position = read_mr_sensor();




  // Convert handle and base angle
  float handle_angle;
  float base_angle;
  float motor_torque;
  int i;


  //pot2Value = 1; // remove later TODO
  calculate_angles(pot1Value,pot2Value, handle_angle, base_angle);




  // === 2. Send structured message over UART ===
  //Serial.print("<P1:");
  //Serial.print(pot1Value);                                                
  //Serial.print(",");
  Serial.print(int(handle_angle));
  Serial.print(",");
  Serial.print(int(base_angle));
  Serial.println("\n");


  // === 3. Check for UART input (10-bit value from sender) ===
  while (Serial.available()) {
    char c = Serial.read();


    if (c == '\n') {
      i = inputBuffer.toInt();         // Parse 10-bit value
      //float currentValueCurrent = (float)receivedRaw;         // Convert to float


      if (!baselineSet) {
        baselineCurrent = i;
        baselineSet = true;
      }
      // Use float value in your function
      //calculate_direction(handle_angle);
      //Serial.print("Function result: ");
      //Serial.println(result, 2);
      //direction_test(handle_angle);


      inputBuffer = "";  // Clear buffer
    } else {
      inputBuffer += c;
    }
  }




  direction_test(handle_angle, i);


  int mappedValue = 0;
  if (baselineSet) {
    mappedValue = customMap(i, baselineCurrent, baselineCurrent + 80, 0, 100);
    mappedValue = constrain(mappedValue, 0, 100);
  }


 


  //int mappedValue = map(receivedRaw,  0, 70, 0, 255);
//
 // if (motor_torque >= 0){
  //  digitalWrite(MOTOR_DIR_PIN, HIGH);
 // }else{
 //   digitalWrite(MOTOR_DIR_PIN, LOW);
 // }
/*
  int k = 100;
  if (handle_angle > 95){
    mappedValue = k*(handle_angle-95);
  }else{
    mappedValue = 0;
  }
mappedValue = constrain(mappedValue, 0, 150); // Safety net




  digitalWrite(MOTOR_DIR_PIN_1, LOW);
  digitalWrite(MOTOR_DIR_PIN_2, HIGH);
  */
  analogWrite(MOTOR_PWM_PIN, mappedValue);
 
 




  //Serial.print(mappedValue);
  //Serial.print("\n");




 // int output;
  //output = command_motor(motor_torque);


 // Serial.print(output);
  //Serial.print("\n");


  delay(50);  // adjust timing TODO
}