/*
  Arduino | hardware-help
  Dc Motor Circuit Problems
  Nick — 6/25 at 5:29 PM
  Hi i am trying to build a circuit which uses a arduino and
  the following components a 1000kv dc brushless motor with
  a 30A esc speed controller, a 3s lipo battery, arduino,
  16x2 screen, hall sensor and bread board.
  I tried making the circuit by hand but the software from
  chatgpt encounterd many problems. I am therefore not sure
  if the following are correct my circuit as i want to be able
  to supply up to 8v to the motor but idk if the arduino is
  limiting it due to the 5v line. If the pulse modulation to
  the motor works as my code could only do max or min and not
  control the power. 3rd the software as my program didnt work.
  Any input on my circuit or program would be greatly appreciated.
  I am sorry for it being so messy
*/

#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
//#include </pulse.chip.c>

Servo esc; // Create a Servo object to control the ESC
//chip_state_t pulseGen;

const int ESC_PIN = 4; // ESC signal pin
const int HALL_PIN = 2; // Hall sensor digital output pin
const int THROTTLE_PIN = A0;
const unsigned long INTERVAL = 1000;

volatile unsigned int hallPulses = 0;
//volatile float accumulatedL = 0;
//volatile float currentRPM = 0; // Current RPM of the motor
//float targetRPM = 2000; // Example target RPM
//float pwmValue = 1050; // Initial PWM signal

unsigned long prevMillis = 0;

// PID constants
float Kp = 0.5;
float Ki = 0.1;
float Kd = 0.05;

float previousError = 0;
float integral = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, adjust if necessary

void initEsc()  {
  // ESC Initialization sequence
  Serial.println("Initializing ESC...");
  lcd.clear();
  lcd.print("Initializing ESC");
  delay(1000);
  esc.writeMicroseconds(500);   // Minimum throttle signal (ESC arm signal)
  //esc.write(0);
  lcd.setCursor(2, 1);
  lcd.print("MIN ");
  delay(1000);
  esc.writeMicroseconds(2500);  // Maximum throttle signal for calibration
  //esc.write(180);
  lcd.print("MAX ");
  delay(1000);
  esc.writeMicroseconds(500);   // Minimum throttle signal
  //esc.write(0);
  lcd.print("MIN");
  delay(1000);
  Serial.println("ESC Initialized");
  lcd.clear();
  lcd.setCursor(4, 0);
  lcd.print("ESC init");
  lcd.setCursor(6, 1);
  lcd.print("done");
  delay(1000);
  lcd.clear();
}

void rpmCounter() {
  hallPulses++;
}

double showRPM()  {
  char buffer[16];
  char fBuffer[8];

  cli();
  double displayRPM = (hallPulses * 60.0) / 1000.0; // 61.1 is fudge factor
  hallPulses = 0;
  sei();
  dtostrf(displayRPM, 3, 2, fBuffer);
  snprintf(buffer, 16, "RPM: %s x1K ", fBuffer);
  Serial.println(buffer);
  lcd.setCursor(0, 0);
  lcd.print(buffer);
  return displayRPM;
}

/*
  void updateAndAdjustPWM() {
  // Calculate current RPM
  currentRPM = (accumulatedL / 10.0) * 6.0; // RPM calculation every second over the last 10 seconds
  accumulatedL = 0; // Reset accumulated L value

  // PID Controller
  float error = targetRPM - currentRPM;
  integral += error * (0.005); // Integral term, 0.005 is the interval in seconds
  float derivative = (error - previousError) / 0.005; // Derivative term
  float output = Kp * error + Ki * integral + Kd * derivative;

  // Adjust PWM value
  pwmValue += constrain(output, -0.5, 0.5); // Ensure PWM value is adjusted smoothly
  pwmValue = constrain(pwmValue, 1000, 2000); // Ensure PWM value is within range
  esc.writeMicroseconds(pwmValue);

  previousError = error;
  }
*/

void setup() {
  Serial.begin(115200);
  lcd.init();
  pinMode(HALL_PIN, INPUT_PULLUP);
  esc.attach(ESC_PIN);
  lcd.backlight();
  // splash screen
  lcd.clear();
  lcd.setCursor(4, 0);
  lcd.print("RPM Test");
  delay(1000);
  lcd.clear();
  // calibrate ESC
  //initEsc();
  attachInterrupt(digitalPinToInterrupt(HALL_PIN), rpmCounter, FALLING);
}

void loop() {
  double rpm = 0;

  if (millis() - prevMillis >= INTERVAL)  {
    prevMillis = millis();
    double tempRpm = showRPM();
    rpm = tempRpm;
    Serial.println(rpm);
    //pulseGen.frequency += 100;
  }
  int spdValue = analogRead(THROTTLE_PIN);
  double target = (map(spdValue, 0, 1023, 0, 192) * 60);

  lcd.setCursor(0, 1);
  lcd.print("SET: ");
  lcd.print(target / 1000.0, 2);
  lcd.print(" x1K ");

  /*
    // Adjust PWM based on target RPM for 40 seconds
    unsigned long currentTime = millis();
    while (millis() - currentTime < 40000) {
    // Display current RPM on LCD
    lcd.setCursor(5, 0);
    lcd.print(currentRPM);
    lcd.print("       "); // Clear any remaining characters

    // Print current RPM to serial monitor
    Serial.print("Current RPM: ");
    Serial.println(currentRPM);
    }

    // Stop the motor
    esc.writeMicroseconds(1000); // Minimum throttle to stop the motor
    Serial.println("Motor stopped");

    // Clear the display
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Motor stopped");

    while (true) {
    // Infinite loop to prevent restarting
    }
  */
}