/*
ESP32 versão WROOM - 32D
Ligação: LDR´s energizados em 5V.
Range de leitura: 0 a 4095.
ADC resolução 12 bits.
*/


#include <ArduTFLite.h>
#include "model.h"

#include <ESP32Servo.h>

//Declare two servos
Servo servo_updown;
Servo servo_rightleft;

const int servoPinElev = 2;
const int servoPinAzi = 4;

int  topl = 0;
int  topr = 0; 
int  botl = 0;
int  botr = 0;

//limites horizontal
int servohori = 45;
int servohoriLimitHigh = 175;
int servohoriLimitLow = 5;
//limites vertical
int servovert = 45;
int servovertLimitHigh = 165;
int servovertLimitLow = 5;

int  threshold_value = 250;     

constexpr int kTensorArenaSize = 5 * 1024;
alignas(16) uint8_t tensor_arena[kTensorArenaSize];


#define ldrtopr 39 // ESP32 pin GIOP39 VN (ADC1_3)   LDR 1   top-right
#define ldrbotr 36 // ESP32 pin GIOP36 VP  (ADC1_0)  LDR 2  bottom-right
#define ldrtopl 35 // ESP32 pin GIOP35 D35 (ADC1_7)  LDR 3 top-left
#define ldrbotl 34 // ESP32 pin GIOP34 D34 (ADC1_6)  LDR 4  bottom-left


void setup() {
  
  Serial.begin(115200);
 	
 	servo_updown.attach(servoPinElev); 
 	servo_rightleft.attach(servoPinAzi); 

  servo_rightleft.write(servohori); delay(200);
  servo_updown.write(servovert); delay(200);
  
  if (!modelInit(model, tensor_arena, kTensorArenaSize)){
    Serial.println("Model initialization failed!");
    while(true);
  }
  Serial.println("Model initialization done.");


}


void loop() {
 
   automaticsolartracker();
   delay(100);
}


void  automaticsolartracker(){
  
    //Leitura LDR
     topr= analogRead(ldrtopr);         
     topl= analogRead(ldrtopl);         
     botr= analogRead(ldrbotr);         
     botl= analogRead(ldrbotl);         
     
    Serial.print("topr: ");    Serial.println(topr);
    Serial.print("topl: ");    Serial.println(topl);
    Serial.print("botr: ");    Serial.println(botr);
    Serial.print("botl: ");    Serial.println(botl);

    modelSetInput(topl,0);   //delay(15);//LDR_Top_Left
    modelSetInput(topr,1);   //delay(15);//LDR_Top_Right
    modelSetInput(botl,2);   //delay(15);//LDR_Bottom_Left
    modelSetInput(botr,3);   //delay(15);//LDR_Bottom_Right

    float diffelev = modelGetOutput(0); //delay(15);  //gerar valor de saída Diff_avg_top_bottom.
    float diffazi = modelGetOutput(1);  //delay(15);  //gerar valor de saída Diff_avg_left_right.
 
   
    // Run inference, and report if an error occurs
    if(!modelRunInference()){

          Serial.println("RunInference Failed!");
          return;
    }

    //left-right movement of  solar tracker
     
      if (abs(diffazi) >= threshold_value){        //Change  position only if light difference is bigger then the threshold_value
       if  (diffazi > 0) {
        
        if (servo_rightleft.read() < servohoriLimitHigh) {
          servohori = ++servohori;
          if (servohori > servohoriLimitHigh) servohori = servohoriLimitHigh;
         
         servo_rightleft.write(servohori);
         Serial.print("servo_Horizontal: ");     Serial.println(servohori);        
        } 

      }
      if (diffazi <  0) {
        if (servo_rightleft.read()  > servohoriLimitLow) {
          servohori = --servohori;
          if (servohori < servohoriLimitLow) servohori = servohoriLimitLow;
         
         servo_rightleft.write(servohori);
         Serial.print("servo_Horizontal: ");     Serial.println(servohori); 
       
        } 
      }
    }
             
      //up-down movement of solar tracker

      if (abs(diffelev) >= threshold_value){    //Change position only if light  difference is bigger then thethreshold_value
       if (diffelev > 0) {
        if  (servo_updown.read() < servovertLimitHigh) {
         
         servovert = ++servovert;

         if (servovert > servovertLimitHigh) servovert = servovertLimitHigh;
         servo_updown.write(servovert);
         Serial.print("servo_vertical: ");     Serial.println(servovert);
        } 
      }
      if (diffelev <  0) {
        if (servo_updown.read()  > servovertLimitLow) {
          servovert = --servovert;        
        
        if (servovert < servovertLimitLow) servovert = servovertLimitLow;
        servo_updown.write(servovert);
        Serial.print("servo_vertical: ");     Serial.println(servovert);
        } 
      }
    }       


}