#include <Servo.h>
#include <PIDController.h>
#include "PID_v1.h"

double v_referencial, perturbacion, InputPID, luzMedida;

//Se especifican los valores del controlador PID
double Kp = 2, Ki = 1, Kd = 25; 

PID myPID(&InputPID, &luzMedida, &v_referencial, Kp, Ki, Kd, P_ON_E, DIRECT);

Servo myServo; // Objeto del servo
int ldrPin = A0; // Pin analógico donde está conectado el LDR
int solPin = A1; // Pin analogico donde se ajusta el angulo del sol
int servoAngle; // Variable para el ángulo del servo
int servoAngleInicial = 30;
#define BUTTON_PIN 2
#define COTA_MIN 30
#define COTA_MAX 40

int kp = 1;

int analogValue;
float voltage;
float resistance;
float luzSol;
int anguloSol;

int analogValueRef;

// These constants should match the photoresistor's "gamma" and "rl10" attributes
const float GAMMA = 0.7;
const float RL10 = 50;

double luzMedidaAnterior;
double anguloServoAnterior;


void setup() {
  // put your setup code here, to run once:
  myServo.attach(9); // Conectar el servo al pin 9
  Serial.begin(9600); // Iniciar comunicación serial
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  /*myPID.SetOutputLimits(0, 255);

  v_referencial = 0;
  perturbacion = 0;
  myPID.SetMode(AUTOMATIC); */

  simularMedicionInicial(); // simular medicion inicial

}

void loop() {
  // put your main code here, to run repeatedly:

  v_referencial = map(analogRead(A2), 0, 1023, COTA_MIN, COTA_MAX);

  anguloSol = lecturaAnguloSol();
  luzSol = constrain(lecturaLuzSol(), 0, 100000);

  luzMedida = obtenerLuzMedida(luzSol, anguloSol, anguloServoAnterior);
  double deltaL = luzMedida - luzMedidaAnterior;

  servoAngle = (int)map(luzMedida, 0, 1053, COTA_MIN, COTA_MAX);

  while (servoAngle != v_referencial) {
    if (deltaL > 0) {
      servoAngle = anguloServoAnterior - (1 * kp);
    } else if (deltaL < 0) {
      servoAngle = anguloServoAnterior + (1 * kp);
    }

    myServo.write(servoAngle);

    anguloServoAnterior = servoAngle;

    //luzMedidaAnterior = luzMedida;
  }
  

  /*servoAngle = map(luzMedida, 0, 1023, 30, 40);

  myServo.write(servoAngle);

  anguloServoAnterior = servoAngle;

  //luzMedidaAnterior = luzMedida;

  InputPID = servoAngle;*/
 

  Serial.print("Valor Referencial de angulo: ");
  Serial.println(v_referencial);
  Serial.print("Angulo del sol: ");
  Serial.println(anguloSol);
  Serial.print("Luz del sol: ");
  Serial.println(luzSol);
  Serial.print("Luz recibida por panel: ");
  Serial.println(luzMedida);
  Serial.print("Angulo del panel: ");
  Serial.println(servoAngle);
  Serial.println("-----------");

  delay(800);

}

int lecturaAnguloSol(){
  int a = map(analogRead(solPin), 0, 1023, 0, 180);
  return a;
}

float lecturaLuzSol(){
  analogValue = analogRead(A0);
  voltage = analogValue / 1024. * 5;
  resistance = 2000 * voltage / (1 - voltage / 5);
  float lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));
  return lux;
}

void simularMedicionInicial() {
  anguloSol = map(analogRead(solPin), 0, 1023, 0, 180);

  analogValue = analogRead(ldrPin);
  voltage = analogValue / 1024. * 5;
  resistance = 2000 * voltage / (1 - voltage / 5);
  luzSol = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));

  luzMedidaAnterior = obtenerLuzMedida(luzSol, anguloSol, servoAngleInicial);

  servoAngle = map(luzMedidaAnterior, 0, 1053, COTA_MIN, COTA_MAX);

  myServo.write(servoAngle);

  anguloServoAnterior = servoAngle;

  //return (float)luzMedidaAnterior;
  //return servoAngle;
}


double obtenerLuzMedida(float luzSolar, int anguloSolar, int anguloPanel){
  //int angulo = map(anguloSolar, 0, 180, 0, 90);

  //double luzRecibida = luzSolar * angulo / 45;

  double luzRecibida = luzSolar * anguloSolar / 45;

  luzRecibida = luzRecibida * cos(radians(anguloPanel));

  return luzRecibida;
}