#include <WiFiClientSecure.h>
#include <PubSubClient.h>
#include <WiFi.h>
#include <ESP32Servo.h>

// WiFi
#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""

//(自動追光時)平滑讀取analogpin使用
//Beta值(0~1)，Beta越小 馬達反應慢(抖動少)，Beta越大 馬達反應快(可能抖動)
#define autotrack_Beta 0.25
//伺服馬達腳位
#define xservo_pin 19
#define yservo_pin 21
//伺服馬達初始角度
#define init_x_angle 90
#define init_y_angle 90
//光敏電阻腳位
#define LDR_L_pin 32
#define LDR_R_pin 33
#define LDR_U_pin 34
#define LDR_D_pin 35

// 馬達設定
Servo xservo;
Servo yservo;
int x_angle = init_x_angle;
int y_angle = init_y_angle;
int last_x_angle = x_angle;
int last_y_angle = y_angle;

// 變數
String app_state = "1";
// 接收MQTT訊息後，把訊息存入變數(追光模式狀態)
String save_auto="false";
String save_direction="0";

// MQTT
WiFiClientSecure wifiClient;
PubSubClient mqttClient(wifiClient);

// 函式宣告
void Tasktrack(void *pvParameters);
void autoTrack();
void moveServo(int x, int y);
int smoothRead(int pin, int lastValue, float Beta = autotrack_Beta);
void callback(char* topic, byte* payload, unsigned int length);

void setup() {
  Serial.begin(115200);
  // WiFi連線
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(200);
    Serial.print(".");
  }
  Serial.println(" Connected!");
  // MQTT連線
  wifiClient.setInsecure(); 
  mqttClient.setServer("e13e9dc2dbcb42729639f6aa71a1770d.s1.eu.hivemq.cloud", 8883); 
  mqttClient.setCallback(callback);
  Serial.println("Connecting MQTT");
  while (!mqttClient.connected()) {
    if (mqttClient.connect("ESP32", "sunflower", "Tony970505@")) {
      // 訂閱
      mqttClient.subscribe("sunflower/control/auto");
      mqttClient.subscribe("sunflower/control/direction");
      mqttClient.subscribe("sunflower/app/state");
      Serial.println("MQTT connected !");
    } else {
      Serial.print("MQTT connection failed:");
      Serial.println(mqttClient.state());
      delay(2000);
    }
  }
  // 馬達初始化
  xservo.attach(xservo_pin);
  yservo.attach(yservo_pin);
  xservo.write(x_angle);
  yservo.write(y_angle);
  // 設定ADC解析度
  analogReadResolution(12);
  // 建立Tasktrack，指定核心 1
  xTaskCreatePinnedToCore(Tasktrack,"Tasktrack",8192,NULL,1,NULL,1);
}

void loop(){
  // MQTT 斷線檢查
  if (!mqttClient.connected()) {
    Serial.println("MQTT disconnected, reconnecting...");
    while (!mqttClient.connected()) {
      if (mqttClient.connect("ESP32", "sunflower", "Tony970505@")) {
        mqttClient.subscribe("sunflower/control/auto");
        mqttClient.subscribe("sunflower/control/direction");
        mqttClient.subscribe("sunflower/app/state");
        Serial.println("MQTT Reconnected!");
      } else {
        Serial.print("MQTT Reconnection failed.:");
        Serial.println(mqttClient.state());
        delay(2000);  
      }
    }
  }
  mqttClient.loop();
}

// 收到 MQTT 訊息
void callback(char* topic, byte* payload, unsigned int length) {
  String msg = "";
  for (int i = 0; i < length; i++) msg += (char)payload[i];
  if(strcmp(topic, "sunflower/control/auto") == 0){
    save_auto=msg;
    Serial.println("Message Auto : " + save_auto);
  }else if(strcmp(topic, "sunflower/control/direction") == 0){
    save_direction=msg;
    Serial.println("Message Direction : " + save_direction);
  }else if(strcmp(topic, "sunflower/app/state") == 0){
    app_state=msg;
    Serial.println("Message APP State : " + app_state);
  }
} 

//每30ms判斷追光模式並控制馬達函式
void Tasktrack(void *Parameters){
 TickType_t trackTime=xTaskGetTickCount();
  while(1){
  if(save_auto =="true"&& save_direction =="0"){
    autoTrack();//自動追光
  }else if(save_auto == "false" && save_direction != "0"){
    // 手動控制
    if(save_direction == "3") moveServo(-2, 0);
    else if(save_direction == "4") moveServo(2, 0);
    else if(save_direction == "1") moveServo(0, -2);
    else if(save_direction == "2") moveServo(0, 2);
    Serial.print("Manual X_angle=");Serial.println(x_angle);
    Serial.print("Manual Y_angle=");Serial.println(y_angle);
    }
  vTaskDelayUntil(&trackTime,pdMS_TO_TICKS(30));
 }
}

//馬達動作函式
void moveServo(int x, int y) {
  // 更新角度
  x_angle = constrain(x_angle + x, 0, 180);
  y_angle = constrain(y_angle + y, 0, 180);
  // 只在角度改變時寫入馬達
  if(x_angle != last_x_angle) xservo.write(x_angle);
  if(y_angle != last_y_angle) yservo.write(y_angle);
  // 更新last_angle
  last_x_angle = x_angle;
  last_y_angle = y_angle;
}

//(自動追光時)平滑讀取analogpin 函式
int smoothRead(int pin, int lastValue, float Beta) {
  int val = lastValue * (1.0 - Beta) + analogRead(pin) * Beta;
  return val;
}

//自動追光函式
void autoTrack(){
  static int L = 0;
  static int R = 0;
  static int U = 0;
  static int D = 0;
  L = smoothRead(LDR_L_pin, L);
  R = smoothRead(LDR_R_pin, R); 
  U = smoothRead(LDR_U_pin, U); 
  D = smoothRead(LDR_D_pin, D); 
  if (!(abs(L - R) < 50 && abs(U - D) < 50)){
  if(R - L > 50) moveServo(2, 0);       // 馬達右轉
  else if(L - R > 50) moveServo(-2, 0); // 馬達左轉
  if(U - D > 50) moveServo(0, 2);       // 馬達上轉
  else if(D - U > 50) moveServo(0, -2); // 馬達下轉
  }
  Serial.print("Auto x_angle=");
  Serial.println(x_angle);
  Serial.print("Auto y_angle=");
  Serial.println(y_angle);
}