#include <ESP32Servo.h>
#include <string.h>
#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
#include<Wire.h>
#include<TFLI2C.h>

uint8_t broadcastAddress[] = {0x7C, 0x9E, 0xBD, 0xF7, 0x9E, 0x88};
typedef struct struct_message {
  float data1=0.00;
  float data2=0.00;
  float data3=0.00;
  float data4=0.00;
  float data5=0.00;
  float data6=0.00;
  float data7=0.00;
  float data8=0.00;
  float data9=0.00;
  float data10=0.00;
  float data11=0.00;
  float data12=0.00;
} struct_message;
struct_message dataDistance;
esp_now_peer_info_t peerInfo;

float highTotal[1000];
int numHigh=0;
int angle1[4] = {0,45,90,135};
int angle2[4] = {0,45,90,135};
Servo mt1;
Servo mt2;
int time_min = 1;
int pinSW=2;
int n=0,m=0;
int sw=0;
int num=0,num2=0;
int n_times=0;
float offset=0.015;
TFLI2C tflI2C;
int numLi=0;
int16_t tfDist;
int16_t tfAddr = TFL_DEF_ADR;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

void goSleep(){
  Serial.println("Going to sleep...");
  delay(5000);
  esp_sleep_enable_timer_wakeup(time_min * 60 * 1000 * 1000);
  esp_deep_sleep_start();

}

void highSave(int x,float h){
  highTotal[x]=h;
}

void setup() {
    Serial.begin(115200);
    // Servo at PIN D0
    mt1.attach(13); 
    mt2.attach(14);   
    dhtSensor.setup(DHT_PIN, DHTesp::DHT22);
    pinMode(pinSW, INPUT); 
    Wire.begin();

    WiFi.mode(WIFI_STA);
    if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_register_send_cb(OnDataSent);
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  
  // Add peer        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }

}

void moter1(int x){
  if(x!=0)
  {
    num+=1;
  }
    Serial.print("SV1: หมุนไป ");
    Serial.print(x);
    Serial.println(" องศา");
    int msec=0;
    msec=1000*(((2.5-0.5)*x/180)+0.5)+offset;
    mt1.writeMicroseconds(msec);
    delay(500);
}
void moter2(int x){
    Serial.print("SV2: หมุนไป ");
    Serial.print(x);
    Serial.println(" องศา");
    int msec=0;
    msec=(1000*(((2.5-0.5)*x/180)+0.5))+offset;
    mt2.writeMicroseconds(msec);
    
    delay(2500);
    if(x!=0){
      if(tflI2C.getData(tfDist, tfAddr)){
      Serial.println(String(tfDist) + " cm.");
      highSave(numHigh,tfDist);
      numHigh++;
      num2+=1;
    delay(1000);
      }  
      else       Serial.println(" not...");

}
}

void loopmt2(int x){
  int a=0;
  sw = digitalRead(pinSW);
  while(a!=1){
  if(sw == 1)
    {
    moter2(x);
    a=1;
      }
  else {
    while(sw!=1){
      Serial.println("SV2: ไม่เริ่มต้นที่ 0 องศา");
      moter2(0);
      sw = digitalRead(pinSW);
      a=0;
    }
  }
  }
  delay(1000);
  moter2(0);
  a=0;
}

void results(){
  Serial.println("*** ผลการทำงาน ***");
  Serial.print("SV1: จำนวนองศาที่หมุน: ");
  Serial.println(num);
  Serial.print("SV2: จำนวนองศาที่หมุน: ");
  Serial.println(num2);
  int k=0;
    for(int j=0;j<=num;j++){
      Serial.println("*** ====== ***");
      Serial.print("Motor1 : ");
      Serial.print(angle1[j]);
      Serial.println(" degree");
      Serial.println("*** ====== ***");
      for(int i=1;i<=num;i++){
      Serial.print("Motor2 : ");
      Serial.print(angle2[i]);
      Serial.println(" degree");
      Serial.print("ความสูงที่ได้ :");
      Serial.println(highTotal[k]);
      k++;
      }
    }
  Serial.println();
  Serial.println("---");
  Serial.println("====================");
}

void loop() {
  for(n=0;n<4;n++){
    moter1(angle1[n]);
    for(m=0;m<4;m++){
    loopmt2(angle2[m]);
    delay(500);
    }
  }
    moter1(0);
    delay(2000);
    n_times+=1;
    results();
  dataDistance.data1 = highTotal[0];
  dataDistance.data2 = highTotal[1];
  dataDistance.data3 = highTotal[2];
  dataDistance.data4 = highTotal[3];
  dataDistance.data5 = highTotal[4];
  dataDistance.data6 = highTotal[5];
  dataDistance.data7 = highTotal[6];
  dataDistance.data8 = highTotal[7];
  dataDistance.data9 = highTotal[8];
  dataDistance.data10 = highTotal[9];
  dataDistance.data11 = highTotal[10];
  dataDistance.data12 = highTotal[11];
    sentData0();
    

    num=0;
    num2=0;
    goSleep();
}

void sentData0(){
    esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &dataDistance, sizeof(dataDistance));
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }
  delay(2000);
}