//cpp
#include <PID_v1_bc.h>

const int appPin1 = 36; // Pin analógico para la señal 1 del pedal
const int appPin2 = 39; // Pin analógico para la señal 2 del pedal
const int tpsPin1 = 34; // Pin analógico para la señal 1 del throttle body
const int tpsPin2 = 35; // Pin analógico para la señal 2 del throttle body
const int motorPin1 = 32; // Pin para la dirección 1 del puente H
const int motorPin2 = 33; // Pin para la dirección 2 del puente H
const int enablePin = 25; // Pin para habilitar el puente H

const int throttlePin = 14; // Pin para la señal de posición de la mariposa
const int rpmPin = 27;      // Pin para la lectura del tacómetro
const int pwmPin = 26;      // Pin PWM para controlar la velocidad del motor

double Setpoint, Input, Output;
double Kp=2, Ki=5, Kd=1; // Constantes del control PID
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(9600);
  pinMode(appPin1, INPUT);
  pinMode(appPin2, INPUT);
  pinMode(tpsPin1, INPUT);
  pinMode(tpsPin2, INPUT);
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(enablePin, OUTPUT);

  pinMode(throttlePin, INPUT);
  pinMode(rpmPin, INPUT);
  pinMode(pwmPin, OUTPUT);
  
  // Iniciar el control PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(100);
  myPID.SetOutputLimits(0, 255);
  
  Setpoint = 650;  // Establecer el ralentí deseado en RPM

  /*configurar frecuencia del motor a 100hz 
  ledcSetup(0,100,8);//canal 0, 100hz, resolucion de 8bits
  ledcAttachPin(motorPin1,0);//asociar el pin con el canal pwm
  ledAttachPin(motorPin2,0);//asociar el pin con el canal PWM*/
}

void loop() {
  // Leer las señales del pedal y del throttle body
  int appSignal1 = analogRead(appPin1);
  int appSignal2 = analogRead(appPin2);
  int tpsSignal1 = analogRead(tpsPin1);
  int tpsSignal2 = analogRead(tpsPin2);

  // Realizar algún tipo de cálculo o algoritmo de control para determinar la señal de salida al motor basándose en las señales del pedal y del throttle body
  int motorSpeed = map((appSignal1 + appSignal2) / 2, 0, 1023, 0, 255);
  int umbral=(appSignal1 + appSignal2) / 2;
  
  // Control del sentido del motor basado en las señales del throttle body
  if (tpsSignal1 > umbral && tpsSignal2 > umbral) {
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
  } else if (tpsSignal1 < umbral && tpsSignal2 < umbral) {
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
  } else {
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
  }

  // Realizar la lectura de la posición de la mariposa
  int throttleValue = analogRead(throttlePin);
  Input = map(throttleValue, 0, 4095, 0, 100); // Mapeo de la posición de la mariposa

  // Calcular la nueva salida del control PID
  myPID.Compute();

  // Control de la velocidad del motor
  analogWrite(enablePin, motorSpeed);

  // Establecer la velocidad del motor con la salida del control PID
  analogWrite(pwmPin, Output);

  // Muestra los valores para realizar seguimiento en el monitor serial
  Serial.print("APP Signal 1: ");
  Serial.print(appSignal1);
  Serial.print("   APP Signal 2: ");
  Serial.print(appSignal2);
  Serial.print("   TPS Signal 1: ");
  Serial.print(tpsSignal1);
  Serial.print("   TPS Signal 2: ");
  Serial.print(tpsSignal2);
  Serial.print("   Motor Speed: ");
  Serial.println(motorSpeed);
  
  delay(100); // Un pequeño retraso para evitar sobrecargar el monitor serial
}