#include <Servo.h>

Servo myServo; // Objeto del servo
int ldrPin = A0; // Pin analógico donde está conectado el LDR
int servoAngle; // Variable para el ángulo del servo
int servoAngleInicial = 50;

int kp = 1; // Constante proporcional

#define BUTTON_PIN_DIS 2
#define BUTTON_PIN_AUM 1
#define TIEMPO_DELAY 1000

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

double valor_ref, valor_referencia, lux, luxPert;
int valor_nominal;

void setup() {
  myServo.attach(9); // Conectar el servo al pin 9
  Serial.begin(9600); // Iniciar comunicación serial
  pinMode(BUTTON_PIN_DIS, INPUT_PULLUP);
  pinMode(BUTTON_PIN_AUM, INPUT_PULLUP);
  pinMode(A1, INPUT);

  simularSituacionInicial();

  //Debido a que, por limitacion de la simulacion, no se puede ajustar el valor del
  //LDR automaticamente en tiempo de ejecucion. 
}

int lastState = HIGH;
int lastState2 = HIGH;
void loop() {

  // Lectura de potenciometro
  valor_ref = analogRead(A1); // Potencia a la que queremos alcanzar -- eleccion de los lux (tita I)
  valor_referencia = mapeoValorReferencia(valor_ref);
  valor_nominal = obtenerValorNominal(valor_referencia);
  valor_referencia = constrain(valor_referencia, 0.00, 100000.00);
  
  // Control de lazo cerrado: ajuste fino del ángulo del panel solar
  double error = valor_referencia - lux;

  // Operacion de control
  if (error > 0){
    servoAngle = servoAngle - (1 * kp);
    lux = lux + 1000.00;
  } else if (error < 0) {
    servoAngle = servoAngle + (1 * kp);
    lux = lux - 1000.00;
  }

  servoAngle = constrain(servoAngle, 0, 180);

  // Control del servo con el ángulo corregido
  myServo.write(servoAngle);

  // Logica para perturbacion disminucion
  int value = digitalRead((BUTTON_PIN_DIS));
  if (lastState != value) {
    lastState = value;

    while (value == LOW) {
      //Si entra al while, existe perturbacion

      // Operatoria para "disminuir" la cantidad de luz que recibe
      luxPert = lux;
      //luxPert -= 200;
      luxPert -= 1000.00;
      if (luxPert < 0) {
        luxPert = 0;
      }

      servoAngle = servoAngle + (1 * kp);

      servoAngle = constrain(servoAngle, 0, 180);

      // Control del servo con el ángulo corregido
      myServo.write(servoAngle);

      lux = luxPert;

      // Mostrar valores en el monitor serial
      Serial.print("PERTURBACION DISMINUCION");
      Serial.print(" - Valor referencia (tita I): ");
      Serial.println(valor_referencia);
      Serial.print(" - LUX: ");
      Serial.println(luxPert);
      Serial.print(" - Angulo del panel solar (servo): ");
      Serial.println(servoAngle);
      Serial.print(" - Valor deseado:");
      Serial.println(valor_nominal);
      Serial.println(" -------- ");

      delay(TIEMPO_DELAY);

      value = digitalRead((BUTTON_PIN_DIS));
    }
  }

  // Logica para perturbacion aumento
  int value2 = digitalRead((BUTTON_PIN_AUM));
  if (lastState2 != value2) {
    lastState2 = value2;

    while (value2 == LOW) {
      //Si entra al while, existe perturbacion

      // Operatoria para "aumentar" la cantidad de luz que recibe
      luxPert = lux;
      //luxPert -= 200;
      luxPert += 1000.00;

      servoAngle = servoAngle - (1 * kp);

      servoAngle = constrain(servoAngle, 0, 180);

      // Control del servo con el ángulo corregido
      myServo.write(servoAngle);

      lux = luxPert;

      // Mostrar valores en el monitor serial
      Serial.println("PERTURBACION AUMENTO");
      Serial.print(" - Valor referencia (tita I): ");
      Serial.println(valor_referencia);
      Serial.print(" - LUX: ");
      Serial.println(luxPert);
      Serial.print(" - Angulo del panel solar (servo): ");
      Serial.println(servoAngle);
      Serial.print(" - Valor deseado:");
      Serial.println(valor_nominal);
      Serial.println(" -------- ");

      delay(TIEMPO_DELAY);

      value2 = digitalRead((BUTTON_PIN_AUM));
    }
  } 

  // Mostrar valores en el monitor serial
  Serial.print(" - Valor referencia (tita I): ");
  Serial.println(valor_referencia);
  Serial.print(" - LUX: ");
  Serial.println(lux);
  Serial.print(" - Angulo del panel solar (servo): ");
  Serial.println(servoAngle); 
  Serial.print(" - Valor deseado:");
  Serial.println(valor_nominal);
  Serial.println(" -------- ");

  delay(TIEMPO_DELAY);
}

void simularSituacionInicial() {
  myServo.write(servoAngleInicial); //Siempre arranca con angulo inicial
  servoAngle = servoAngleInicial;
  lux = 50000.00; //Medicion inicial simulada
}

double mapeoValorReferencia(double valorNom) {
  if (valorNom < 100.00) {
    return 10000.00;
  } else if (valorNom >= 100.00 && valorNom < 200.00) {
    return 20000.00;
  }
  else if (valorNom >=200.00 && valorNom < 300.00) {
    return 30000.00;
  }
  else if (valorNom >= 300.00 && valorNom < 400.00) {
    return 40000.00;
  }
  else if (valorNom >= 400.00 && valorNom < 500.00) {
    return 50000.00;
  }
  else if (valorNom >= 500.00 && valorNom < 600.00) {
    return 60000.00;
  }
  else if (valorNom >= 600.00 && valorNom < 700.00) {
    return 70000.00;
  }
  else if (valorNom >= 700.00 && valorNom < 800.00) {
    return 80000.00;
  }
  else if (valorNom >= 800.00 && valorNom < 900.00) {
    return 90000.00;
  }
  else if (valorNom >= 900.00 && valorNom < 1000.00) {
    return 100000.00;
  } else {
    return 100000.00;
  }
}

int obtenerValorNominal(double valorRef) {
  return (int)((100000.00 - valorRef) / 1000);
}