#include <Arduino.h>
#include "mapdata.h"
#include "espnow.h"
#include "ws2812_FASTLED.h"
#include "wemos_oled.h"     //注意可能需要改屏幕的地址0x3C或0x3D等，若屏幕不亮可以试试用wire库中的示例程序扫描地址
#include "esp_wifi.h"        // 添加WiFi库以设置信道

// 函数声明
// void coordinates_to_Leds();
// void joy1_map_led();
// void joy2_map_led();
// void joy1_led_blink();
// void joy2_led_blink();
void zero_test();
void eeprom_ini();
void eeprom_broadcastAddress();

unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis3 = 0;
const long LEDdelay=40;               //(milliseconds)LED变化时间间隔
int fade = 1;  //呼吸效果


  //摇杆坐标转化为离原点距离
  int J1_dis = 0;
  int J2_dis = 0;
  //摇杆坐标转化为0-360的角度
  int J1_angle = 0;  
  int J2_angle = 0;

  //摇杆角度转化为左右摇杆LED序号
  int J1_led_num = 0;
  int J2_led_num = 0;
  //摇杆离原点距离转化为左右摇杆LED亮度
  int J1_led_bright = 0;
  int J2_led_bright = 0; 

//**********************Setup*************************
void setup() {    
  pinmode_pullup();
  FastLED_ini();          //FastLED初始化  
  Serial.begin(115200); 
  delay(100);
  eeprom_ini();   //EEPROM初始化
  Serial.print(" LX_zero: ");Serial.print(EEPROM.read(1));
  Serial.print(" LY_zero: ");Serial.print(EEPROM.read(2));
  Serial.print(" RX_zero: ");Serial.print(EEPROM.read(3));
  Serial.print(" RY_zero: ");Serial.println(EEPROM.read(4)); 
  
  // 初始化 ESP-NOW
  Serial.println("Initializing ESP-NOW...");
  delay(100);
  WiFi.mode(WIFI_STA);

  // 设置WiFi信道为6
  esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
  Serial.println("Set WiFi channel to 6");
  
  Serial.println("InitESPNow");
  // This is the mac address of the Master in Station Mode
  Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
  // Init ESPNow with a fallback logic
  InitESPNow();

  // 设置发送数据回调函数
  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);
  
  // 设置接收数据回调函数
  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info.
//  esp_now_register_recv_cb(OnDataRecv);

  //配对连接pair with another ESP-NOW device
//  pair_device();

  BroadcastMode();//启动时默认为广播模式

//  // In the loop we scan for slave
//  ScanForSlave();
//  // If Slave is found, it would be populate in `slave` variable
//  // We will check if `slave` is defined and then we proceed further
//  
  // `slave` is defined
  // Add slave as peer if it has not been added already
  manageSlave();
  // pair success or already paired         
  
  resetData();
  // Serial.print("Data Size: "); Serial.print(sizeof(data));Serial.println(" Bytes");
  // Serial.print(" LX: ");Serial.print(data.j1PotX);
  // Serial.print(" LY: ");Serial.print(data.j1PotY);
  // Serial.print(" RX: ");Serial.print(data.j2PotX);
  // Serial.print(" RY: ");Serial.print(data.j2PotY);
  // Serial.print(" // R1: ");Serial.print(data.buttonR1);
  // Serial.print(" R2: ");Serial.print(data.buttonR2);
  // Serial.print(" J1: ");Serial.print(data.j1Button);
  // Serial.print(" J2: ");Serial.print(data.j2Button);
  // Serial.print(" LB: ");Serial.print(data.buttonLB);
  // Serial.print(" RB: ");Serial.print(data.buttonRB);
  // Serial.print(" T1: ");Serial.println(data.tSwitch1);

  run_OLED_on_Core_0();//在CORE0上运行OLED相关程序避免延迟影响
     
}

void loop() {
   
  read_joydata();  
  
/*********************************************************************************************/

  LX_to_send = map_normal(LX_read, 0, LX_zero, 255, LX_inverted);    
  LY_to_send = map_normal(LY_read, 0, LY_zero, 255, LY_inverted);     
  RX_to_send = map_normal(RX_read, 0, RX_zero, 255, RX_inverted);    
  RY_to_send = map_normal(RY_read, 0, RY_zero, 255, RY_inverted);    
  
/*********************************************************************************************/  
     
  LX_to_send = constrain(LX_to_send,0,255);
  LY_to_send = constrain(LY_to_send,0,255);
  RX_to_send = constrain(RX_to_send,0,255);  
  RY_to_send = constrain(RY_to_send,0,255);
  
  data.j1PotX = LX_to_send;
  data.j1PotY = LY_to_send;
  data.j2PotX = RX_to_send;  
  data.j2PotY = RY_to_send;   
  data.buttonR1 = digitalRead(R1);
  data.buttonR2 = digitalRead(R2);
  data.j1Button = digitalRead(LS);
  data.j2Button = digitalRead(RS);
  data.buttonLB = digitalRead(LB);
  data.buttonRB = digitalRead(RB);
  data.tSwitch1 = digitalRead(BK);
  
//*****************************************************  
  unsigned long currentMillis = millis();    
  if (currentMillis - previousMillis1 >= LEDdelay) {    
    previousMillis1 = currentMillis;
        
    runLED();

    if (J1_dis<10 && J2_dis<10){
      if (voltage > 3.50){    //电池电压高于3.5V时星闪模式
        // joy1_led_blink();
        // joy2_led_blink(); 
      } else {              //否则省电关灯
        fill_solid (leds1,NUM_LEDS,CRGB::Black);     //关灯
        fill_solid (leds2,NUM_LEDS,CRGB::Black);     //关灯
        FastLED.show();  
      }
    } else {
      // joy1_map_led();
      // joy2_map_led();
    }    
    
  }
    
  if (currentMillis - previousMillis2 >= 100) {//每100ms发送一次
    previousMillis2 = currentMillis;
   // ESP-NOW发送数据
    sendData();
  } 
  
  if (currentMillis - previousMillis3 >= 5000) {//每5秒更新一次电量
    previousMillis3 = currentMillis;
    batteryLevel();         //读电池电量
  } 
  
}