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

double v_referencial, perturbacion, InputPID, lux;

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

PID myPID(&InputPID, &lux, &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

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

int analogValueRef;
float voltageRef;
float resistanceRef;
float luzSolRef;

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

int luzMedidaAnterior, 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);

  InputPID = simularMedicionInicial(); // simular medicion inicial

}

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

  if (myPID.Compute())
  {
    analogValueRef = analogRead(A2);
    voltageRef = analogValueRef / 1024. * 5;
    resistanceRef = 2000 * voltageRef / (1 - voltageRef / 5);
    luzSolRef = pow(RL10 * 1e3 * pow(10, GAMMA) / resistanceRef, (1 / GAMMA));
    v_referencial = map(luzSolRef, 0, 100916.51, 30, 40);

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

    int luzMedida = obtenerLuzMedida(luzMedidaAnterior, anguloSol, anguloServoAnterior);
    int luzPerturbacion = simularPerturbacion(luzMedida);
    int luzRecibida = obtenerLuzMedida(luzPerturbacion, anguloSol, anguloServoAnterior);

    servoAngle = map(luzRecibida, 0, 100916.51, 30, 40);

    myServo.write(servoAngle);

    anguloServoAnterior = servoAngle;

    luzMedidaAnterior = luzRecibida;

    InputPID = (float)luzRecibida;
    //int deltaL = luzMedida - luzMedidaAnterior;
  }

  Serial.print("Valor Referencial de angulo: ");
  Serial.println(v_referencial);
  Serial.print("Angulo del sol: ");
  Serial.println(anguloSol);
  Serial.print("Luz recibida: ");
  Serial.println(InputPID);
  //Serial.print("Angulo del panel: ");
  //Serial.println(servoAngle);

  delay(3000);

}

int simularPerturbacion(int luzMedida) {
  int value = digitalRead((BUTTON_PIN));
  if (value == LOW) {
    luzMedida -= 2000;
  }
  return luzMedida;
}

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

  analogValue = analogRead(A2);
  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, 100916.51, 30, 40);

  myServo.write(servoAngle);

  anguloServoAnterior = servoAngle;

  return (float)luzMedidaAnterior;
}


int obtenerLuzMedida(float luzSolar, int anguloSolar, int anguloPanel){
  float anguloSolarRad = radians(anguloSolar);
  float anguloPanelRad = radians(anguloPanel);

  float cantLuzRecibida = luzSolar * cos(anguloSolarRad - anguloPanelRad);

  return round(cantLuzRecibida);
}

/*int obtenerAnguloReferencial(float luzSolar, int anguloSolar) {

}*/

double obtenerValorReferencial(int luzLDR) {
  // Valores de referencia del angulo de servomotor dado la cantidad de Lux --> Valores inventados!
  if (luzLDR < 10000) {
    return 30.0;
  } else if (luzLDR >= 10000 && luzLDR < 20000) {
    return 30.0;
  } else if (luzLDR >= 20000 && luzLDR < 30000) {
    return 31.0;
  } else if (luzLDR >= 20000 && luzLDR < 30000) {
    return 32.0;
  } else if (luzLDR >= 30000 && luzLDR < 40000) {
    return 33.0;
  } else if (luzLDR >= 40000 && luzLDR < 50000) {
    return 34.0;
  } else if (luzLDR >= 50000 && luzLDR < 60000) {
    return 35.0;
  } else if (luzLDR >= 60000 && luzLDR < 70000) {
    return 36.0;
  } else if (luzLDR >= 70000 && luzLDR < 80000) {
    return 37.0;
  } else if (luzLDR >= 80000 && luzLDR < 90000) {
    return 38.0;
  } else if (luzLDR >= 90000 && luzLDR < 100000) {
    return 39.0;
  } else if (luzLDR >= 100000) {
    return 40.0;
  }
}