#include <ArduinoJson.h>
StaticJsonDocument<256> jsonCmdReceive;
StaticJsonDocument<256> jsonInfoSend;
StaticJsonDocument<256> jsonInfoHttp;
JsonDocument jsonFeedback;
#include <SCServo.h>
#include <Preferences.h>
#include <nvs_flash.h>
#include <esp_system.h>
#include <LittleFS.h>
#include <WiFi.h>
#include <INA219_WE.h>
#include <esp_wifi.h>
#include <WebServer.h>
#include <esp_now.h>
#include <nvs_flash.h>

// functions for barrery info.
#include "battery_ctrl.h"
// functions for oled.
#include "oled_ctrl.h"

// functions for RoArm-M3 ctrl.
#include "RoArm-M3_module.h"

// functions for pneumatic modules and lights ctrl. 
#include "switch_module.h"

// define json cmd.
#include "json_cmd.h"

// functions for editing the files in flash.
#include "files_ctrl.h"

// advance functions for RoArm-M3 ctrl.
#include "RoArm-M3_advance.h"

// functions for wifi ctrl.
#include "wifi_ctrl.h"

// functions for esp-now.
#include "esp_now_ctrl.h"

// functions for uart json ctrl.
#include "uart_ctrl.h"

// functions for http & web server.
#include "http_server.h"

TaskHandle_t serialTaskHandle = NULL;
void setup() {
  cmdQueue = xQueueCreate(CMD_QUEUE_LEN, CMD_MAX_LEN);
  
  espNowMessage.magic = 0xA5A5A5A5;// 固定值 0xA5A5A5A5，保证是合法数据
  Serial.begin(115200);
  Wire.begin(S_SDA, S_SCL);
  while(!Serial) {}

  delay(1200);

  initOLED();
  screenLine_0 = "RoArm-M3";
  screenLine_1 = "version: 0.84";
  screenLine_2 = "starting...";
  screenLine_3 = "";
  oled_update();

  // init the littleFS funcs in files_ctrl.h
  screenLine_2 = screenLine_3;
  screenLine_3 = "Initialize LittleFS";
  oled_update();
  if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");}
  initFS();

  // init the funcs in switch_module.h
  screenLine_2 = screenLine_3;
  screenLine_3 = "Initialize 12V-switch ctrl";
  oled_update();
  if(InfoPrint == 1){Serial.println("Initialize the pins used for 12V-switch ctrl.");}
  switchPinInit();

  // servos power up
  screenLine_2 = screenLine_3;
  screenLine_3 = "Power up the servos";
  oled_update();
  if(InfoPrint == 1){Serial.println("Power up the servos.");}
  delay(500);

  ina219_init();
  inaDataUpdate();
  // init servo ctrl functions.
  screenLine_2 = screenLine_3;
  screenLine_3 = "ServoCtrl init UART2TTL...";
  oled_update();
  if(InfoPrint == 1){Serial.println("ServoCtrl init UART2TTL...");}
  RoArmM3_servoInit();

  // check the status of the servos.
  screenLine_2 = screenLine_3;
  screenLine_3 = "Bus servos status check...";
  oled_update();
  if(InfoPrint == 1){Serial.println("Bus servos status check...");}
  RoArmM3_initCheck(false);

  if(InfoPrint == 1 && RoArmM3_initCheckSucceed){
    Serial.println("All bus servos status checked.");
  }
  if(RoArmM3_initCheckSucceed) {
    screenLine_2 = "Bus servos: succeed";
  } else {
    screenLine_2 = "Bus servos: " + 
    servoFeedback[BASE_SERVO_ID - 11].status +
    servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].status +
    servoFeedback[SHOULDER_DRIVEN_SERVO_ID - 11].status +
    servoFeedback[ELBOW_SERVO_ID - 11].status +
    servoFeedback[GRIPPER_SERVO_ID - 11].status;
  }
  screenLine_3 = ">>> Moving to init pos...";
  oled_update();
  RoArmM3_resetPID();
  RoArmM3_moveInit();

  screenLine_3 = "Reset joint torque to ST_TORQUE_MAX";
  oled_update();
  if(InfoPrint == 1){Serial.println("Reset joint torque to ST_TORQUE_MAX.");}
  RoArmM3_dynamicAdaptation(0, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX);

  screenLine_3 = "WiFi init";
  oled_update();
  if(InfoPrint == 1){Serial.println("WiFi init.");}
  initWifi();

  screenLine_3 = "http & web init";
  oled_update();
  if(InfoPrint == 1){Serial.println("http & web init.");}
  initHttpWebServer();

  screenLine_3 = "ESP-NOW init";
  oled_update();
  if(InfoPrint == 1){Serial.println("ESP-NOW init.");}
  initEspNow();

  screenLine_3_1 = "RoArm-M3 ";
  oled_update();
  if(InfoPrint == 1){Serial.println("RoArm-M3 started.");}

  getThisDevMacAddress();

  updateOledWifiInfo();

  screenLine_2 = String("MAC:") + macToString(thisDevMac);
  oled_update();

  if(InfoPrint == 1){Serial.println("Application initialization settings.");}
  createMission("boot", "these cmds run automatically at boot.");
  missionPlay("boot", 1);

  RoArmM3_handTorqueCtrl(300);

  xTaskCreatePinnedToCore(
    serialTask,           // 任务函数
    "SerialTask",         // 名称
    4096,                 // 栈大小
    NULL,                 // 参数
    5,                    // 优先级
    &serialTaskHandle,    // 任务句柄
    0                     // Core0
  );
 // RoArmM3_allPosAbsBesselCtrl(l2B + l3A + ARM_L4_LENGTH_MM_A, 0, l2A - ARM_L4_LENGTH_MM_B, 0, 0, 3.14, 0.25);
}


void loop() {
  //serialCtrl();
 // server.handleClient();

  char cmdBuffer[CMD_MAX_LEN];

    if (xQueueReceive(cmdQueue,cmdBuffer,0) == pdPASS)
    {
        jsonCmdReceive.clear();

        DeserializationError err =deserializeJson(jsonCmdReceive,cmdBuffer);
        if (!err)
        {
            jsonCmdReceiveHandler();
        }
        serializeJson(jsonInfoHttp, jsonFeedbackWeb);
        ws.textAll(jsonFeedbackWeb);
        //request->send(200, "application/json", jsonFeedbackWeb);
    // 清理
    jsonFeedbackWeb = "";
    }


  unsigned long curr_time = millis();
  if (curr_time - prev_time >= 10){
    constantHandle();
    prev_time = curr_time;
  }

  RoArmM3_getPosByServoFeedback();
  Stall_detection();
  oledInfoUpdate();
  pushTelemetry();
  handleWifiConnection();
  // esp-now flow ctrl as a flow-leader.
  switch(espNowMode) {
  case 1: espNowGroupDevsFlowCtrl();break;
  case 2: espNowSingleDevFlowCtrl();break;
  }

  if (InfoPrint == 2) {
    RoArmM3_infoFeedback();
  }

  if(runNewJsonCmd) {
    jsonCmdReceiveHandler();
    jsonCmdReceive.clear();
    runNewJsonCmd = false;
  }
}