#include <Arduino.h>
#include "Wire.h"
#include "MPU6050.h"
#include <MadgwickAHRS.h>
#include <Adafruit_NeoPixel.h>
#include "EEPROM.h"
#include "DFRobotDFPlayerMini.h"       //串口通讯
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

#if (defined(ARDUINO_AVR_UNO) || defined(ESP8266))   // Using a soft serial port
#include <SoftwareSerial.h>
SoftwareSerial softSerial(/*rx =*/6, /*tx =*/7);
#define FPSerial softSerial
#else
#define FPSerial Serial1
#endif
DFRobotDFPlayerMini myDFPlayer;

// Which pin on the Arduino is connected to the NeoPixels?
// On a Trinket or Gemma we suggest changing this to 1:
#define LED_PIN    10

// How many NeoPixels are attached to the Arduino?
#define LED_COUNT    60

// Declare our NeoPixel strip object:
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);


#define BTN           2
#define BTN_LED       3
#define VOLT_PIN      4

#define MAXBRIGHT     200

#define DEBUG  1
#define HUM_TIMEOUT    30228     //hum1.wav 刷新时间

#define BTN_TIMEOUT 1000     // 按键刷新时间

#define SWING_TIMEOUT 1000   // 摆动刷新时间
#define SWING_L_THR 180      // swing 角度阈值
#define SWING_THR 360        // fast swing 角度阈值
#define STRIKE_THR 100       // hit 加速度阈值    
#define STRIKE_S_THR 320     // hard hit 加速度阈值 
#define FLASH_DELAY 15       // 敲击延时
#define OPEN_THR   160       // 开机阈值

#define PULSE_ALLOW 1       //脉冲使能
#define PULSE_AMPL 10       //脉冲幅度      
#define PULSE_DELAY 30      //脉冲间隔

#define R1 100000            //分压电阻 
#define R2 100000            //分压电阻
#define BATTERY_SAFE 0      //电池监测(1 - allow, 0 - disallow)   
      
MPU6050 accelgyro;          //mpu6050
int16_t ax, ay, az;
int16_t gx, gy, gz;
unsigned long ACC, GYR;
int gyroX, gyroY, gyroZ, accelX, accelY, accelZ;
int16_t Lastgx,Lastgy,Lastgz,Lastax,Lastay,Lastaz;
int Diffgx,Diffgy,Diffgz,Diffax,Diffay,Diffaz;

float k = 0.2;
unsigned long humTimer = -HUM_TIMEOUT, mpuTimer, gestureTimer;    //上次时间计数
unsigned long btn_timer, PULSE_timer, swing_timer, swing_timeout,strike_timeout, battery_timer,FLASH_timer,hit_timer;   //btn_timer：按键更新时间 PULSE_timer：脉冲更新时间 swing_timer：摆动更新时间 swing_timeout：摆动更新 battery_timer：电池更新时间 FLASH_timer 灯带更新时间 hit_timer 敲击更新时间 
boolean ls_chg_state, ls_state;                               // ls_chg_state:按键切换状态   ls_state:开关状态 
boolean bzzz_flag , btnState, btn_flag, hold_flag;            //bzzz_flag:喇叭状态  btnState:按键状态  btn_flag：按键标志 hold_flag：按住标志
boolean lighton_flag, lightoff_flag,hit_flag;                 //lighton_flag: 灯带开机标志  lightoff_flag：灯带关机标志   hit_flag：敲击标志
byte btn_counter,light_counter;                               //btn_counter ：按键计数器  light_counter：灯带更新计数器
byte nowNumber, nowColor;                                     //nowNumber:当前音频 nowColor:当前颜色   
byte red, green, blue, redOffset, greenOffset, blueOffset;    //RGB
boolean eeprom_flag, swing_flag, swing_allow, strike_flag;    //eeprom_flag:e2p存储标志位 swing_flag：晃动标志位 swing_allow：未使用 strike_flag：敲击标志位 
float voltage;                                                //电压
int PULSEOffset;                                              //脉冲偏置
char BUFFER[20];

#define IMU_CAL_COUNTER  (1<<3)
uint32_t ges_counter = 0;                                     //手势计数器
float rolls[IMU_CAL_COUNTER] = {0};                           //翻滚角缓存                      
float pitchs[IMU_CAL_COUNTER] = {0};                          //俯仰角缓存          
Madgwick filter;

BLEServer *pServer = NULL;
BLECharacteristic *pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;
int in_time[2] = { 1586, 786};
int out_time[2] = { 2145, 1770};
int clsh_time[10] = {765,610,644,477,682,547,725,734,878,584};
int swng_time[15] = {591,767,685,893,900,1273,626,546,400,901,521,767,403,893,434};
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"  // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer *pServer) {
    deviceConnected = true;
  };

  void onDisconnect(BLEServer *pServer) {
    deviceConnected = false;
  }
};

class MyCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String rxValue = pCharacteristic->getValue();

    if (rxValue.length() > 0) {
      Serial.println("*********");
      Serial.print("Received Value: ");
      for (int i = 0; i < rxValue.length(); i++) {
        Serial.print(rxValue[i]);
      }

      Serial.println();
      Serial.println("*********");
    }
  }
};
void getMpu6050();
void on_off_sound();
void setAll(byte red, byte green, byte blue);
void setColor(byte color);
void randomPULSE();
void batteryTick();
byte voltage_measure();
void btnTick();
void create_lighton_action();
void create_lightoff_action();
void create_hit_action();
void strikeTick();
void swingTick();
void gestureTick();
void lightTick();
void setup() {
  Serial.begin(115200);
  delay(1000);

  // ---- 按键初始化 ----
  pinMode(BTN, INPUT_PULLUP);
  pinMode(BTN_LED, OUTPUT);
  pinMode(VOLT_PIN,INPUT);
  digitalWrite(BTN_LED, 1);

  Serial.begin(115200);

  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  strip.setBrightness(100); // Set BRIGHTNESS to about 1/5 (max = 255)
  strip.show();            // Turn OFF all pixels ASAP

  delay(200);
  Wire.begin();
  #if (defined ESP32)
    FPSerial.begin(9600, SERIAL_8N1, /*rx =*/6, /*tx =*/7);
  #else
    FPSerial.begin(9600);
  #endif
  accelgyro.initialize();
  accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);   
  if (DEBUG) {
    if (accelgyro.testConnection()) Serial.println(F("MPU6050 OK"));
    else Serial.println(F("MPU6050 fail"));
  }
  filter.begin(1000000/50);

  Serial.println();
  Serial.println(F("DFRobot DFPlayer Mini Demo"));
  Serial.println(F("Initializing DFPlayer ... (May take 3~5 seconds)"));

  if (!myDFPlayer.begin(FPSerial, /*isACK = */true, /*doReset = */true)) {  //Use serial to communicate with mp3.
    Serial.println(F("Unable to begin:"));
    Serial.println(F("1.Please recheck the connection!"));
    Serial.println(F("2.Please insert the SD card!"));
    while(true){
      delay(0); // Code to compatible with ESP8266 watch dog.
    }
  }
  Serial.println(F("DFPlayer Mini online."));

  // Create the BLE Device
  BLEDevice::init("FUN SABER");

  // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pTxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_TX, BLECharacteristic::PROPERTY_NOTIFY);

  pTxCharacteristic->addDescriptor(new BLE2902());

  BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE);

  pRxCharacteristic->setCallbacks(new MyCallbacks());

  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  Serial.println("等待蓝牙客户端连接...");
  myDFPlayer.volume(20);  //Set volume value. From 0 to 30
  
  if(!EEPROM.begin(10))
  {
    if (DEBUG) Serial.println(F("EEPROM init fail"));
    nowColor = 0;
  }
  else
  {
    if (DEBUG) Serial.println(F("EEPROM init OK"));
    if ((EEPROM.read(0) >= 0) && (EEPROM.read(0) <= 2)) {  // check first start
      nowColor = EEPROM.read(0);   // remember color
    } else {                       // first start
      EEPROM.write(0, 0);          // set default
      EEPROM.commit();
      nowColor = 0;                // set default
    }
  }
  setColor(nowColor);
  strip.setBrightness(MAXBRIGHT);
  if (DEBUG) Serial.println("光剑开始运行");
  delay(2000);
}

void loop() {
  getMpu6050();
  btnTick();
  on_off_sound();
  randomPULSE();
  strikeTick();
  swingTick();
  gestureTick();
  lightTick();
  batteryTick();
  if (deviceConnected) {
    pTxCharacteristic->setValue(&txValue, 1);
    pTxCharacteristic->notify();
    txValue++;
    delay(10);  // bluetooth stack will go into congestion, if too many packets are sent
  }

  // disconnecting
  if (!deviceConnected && oldDeviceConnected) {
    delay(500);                   // give the bluetooth stack the chance to get things ready
    pServer->startAdvertising();  // restart advertising
    Serial.println("start advertising");
    oldDeviceConnected = deviceConnected;
  }
  // connecting
  if (deviceConnected && !oldDeviceConnected) {
    // do stuff here on connecting
    oldDeviceConnected = deviceConnected;
  }
}
//获取陀螺仪数据
void getMpu6050() {
  if (ls_state) 
  {                                               // if saber is on
    if (millis() - mpuTimer > 200) {                            
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);       

      // find absolute and divide on 100
      gyroX = abs(gx / 100);
      gyroY = abs(gy / 100);
      gyroZ = abs(gz / 100);
      accelX = abs(ax / 100);
      accelY = abs(ay / 100);
      accelZ = abs(az / 100);

      // vector sum
      ACC = sq((long)accelX) + sq((long)accelY) + sq((long)accelZ);
      ACC = sqrt(ACC);
      GYR = sq((long)gyroX) + sq((long)gyroY) + sq((long)gyroZ);
      GYR = sqrt((long)GYR/2);
      mpuTimer = millis();
    }
  }
}

//手势状态
void gestureTick()
{ 
    static int16_t count_open = 0;
  //  static int16_t count_change = 0; 
    float roll_min,roll_max;
    float pitch_min,pitch_max; 

   if(millis() - gestureTimer > 50)
   {
      gestureTimer = millis();
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 

      float accX = (float)ax/2048.0;     //(32768.0/16)
      float accY = (float)ay/2048.0;
      float accZ = (float)az/2048.0;

      float gyroX = (float)gx/131.0;     //32768.0/250
      float gyroY = (float)gy/131.0;
      float gyroZ = (float)gz/131.0;

      // float roll,pitch,yaw;
      // filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ);
      // // print the heading, pitch and roll
      // roll = filter.getRoll();
      // pitch = filter.getPitch();
      // yaw = filter.getYaw();

      rolls[ges_counter & (IMU_CAL_COUNTER-1)] = atan2(accY,accZ)*57.2974;
      pitchs[ges_counter & (IMU_CAL_COUNTER-1)] = atan2(-accX,sqrt(accY * accY + accZ * accZ))*57.2974;

      roll_min = pitch_min = 10000;
      roll_max = pitch_max = -10000;
      for(int i=0;i<IMU_CAL_COUNTER;i++)
      {
          roll_min =  rolls[i] < roll_min ? rolls[i] : roll_min;
          roll_max = rolls[i] > roll_max ? rolls[i] : roll_max;
          pitch_min =  pitchs[i] < pitch_min ? pitchs[i] : pitch_min;
          pitch_max = pitchs[i] > pitch_max ? pitchs[i] : pitch_max; 
      }

      if(pitch_max - pitch_min > OPEN_THR && count_open == 0)
      {
        ls_chg_state = 1;
        count_open = 40;
        // Serial.println("Roll P-P = ");
        // Serial.println(roll_max - roll_min);
        // Serial.println("Pitch P-P = ");
        // Serial.println(pitch_max - pitch_min);
      }

      ges_counter ++ ;

      if(count_open > 0) count_open--;
      // if(count_change > 0) count_change--;
   }   
}

//按键状态函数
void btnTick() {
  btnState = !digitalRead(BTN);                        //获取按键状态
  if (btnState && !btn_flag)                           //按下                                           
  {                                  
    if (DEBUG) Serial.println(F("BTN PRESS"));
    btn_flag = 1;                                      //按下的状态
    btn_counter++;                                      
    btn_timer = millis();                              //按下的时间
  }
  if (!btnState && btn_flag) {                         //按键释放
    btn_flag = 0;
    hold_flag = 0;                                                                 
  }
  //长按 开关光剑
  if (btn_flag && btnState && (millis() - btn_timer > BTN_TIMEOUT) && !hold_flag) 
  {
    ls_chg_state = 1;                     // flag to change saber state (on/off)
    hold_flag = 1;                        
  }
  //连按 触发
  if ((millis() - btn_timer > BTN_TIMEOUT) && (btn_counter != 0)) {
    if (ls_state) {
       if (DEBUG) Serial.println("Change color");
      if (btn_counter >= 3) {               // 3 press count
        nowColor++;                         // change color
        if (nowColor >= 3) nowColor = 0;
        setAll(red, green, blue);
        eeprom_flag = 1;
      }
    }
    btn_counter = 0;
  }
}

//声音开关 切换完执行一次
void on_off_sound() {
if (ls_chg_state) {                // if change flag
    if (!ls_state)                   // if Saber is turned off   
    {
      if (voltage_measure() > 10 || !BATTERY_SAFE) 
      {
        if (DEBUG) Serial.println(F("SABER ON"));
        // TODO 光剑开启音效   
        myDFPlayer.play(1);                                                                        
        humTimer = millis() - HUM_TIMEOUT + out_time[0];    //-9000是因为HUM.wav是9000ms                               
        create_lighton_action();
        ls_state = 1;             // remember that turned on
        bzzz_flag = 1;
      } 
      else 
      {
        if (DEBUG) Serial.println(F("LOW VOLTAGE!"));
        for (int i = 0; i < 5; i++) {
          digitalWrite(BTN_LED, 0);
          delay(400);
          digitalWrite(BTN_LED, 1);
          delay(400);
        }
      }
    } else {                         // turned on
      bzzz_flag = 0; 
      nowNumber = esp_random() % 2 ;
      // TODO 光剑开启音效   
      myDFPlayer.play(nowNumber);    
      humTimer = millis() - HUM_TIMEOUT + in_time[1];    //-9000是因为HUM.wav是9000ms       
      create_lightoff_action();           
      if (DEBUG) Serial.println(F("SABER OFF"));
      ls_state = 0;               
      if (eeprom_flag) 
      {
        eeprom_flag = 0;
        EEPROM.write(0, nowColor);   // write color in EEPROM
        EEPROM.commit();
      }
    }
    ls_chg_state = 0;
  }

  //SD卡音频播放
  if ((millis() - humTimer) > HUM_TIMEOUT && bzzz_flag) {
    // TODO 光剑开启音效   
    myDFPlayer.play(1);                  //
    humTimer = millis();
    swing_flag = 1;
    strike_flag = 0;
  }  
}

//敲击动作
void strikeTick() {

  if ((ACC > STRIKE_THR) && (ACC < STRIKE_S_THR)&&(millis() - strike_timeout > 500)&& ls_state && lighton_flag == 0 && lightoff_flag == 0) { 
    strike_timeout = millis();
    nowNumber = esp_random() % 2;
    // TODO 光剑敲击音效   
    if (DEBUG) Serial.println("敲击");
    myDFPlayer.play(nowNumber);                                                                             
    humTimer = millis() - HUM_TIMEOUT + clsh_time[nowNumber];                                                                   //敲击标志位
    create_hit_action();
    strike_flag = 1;                                //播放完以后  strike_flag归零
  }

}

//摆动动作  
void swingTick() {
  if (GYR > 120 && (millis() - swing_timeout > 200)&& ls_state && lighton_flag == 0 && lightoff_flag == 0)     //200ms更新一次
  {
    swing_timeout = millis();
    if (((millis() - swing_timer) > SWING_TIMEOUT) && swing_flag && !strike_flag) {
      if (DEBUG) Serial.println("摆动");
      if (GYR >= SWING_THR) {                             //swing               
        nowNumber = esp_random() % 15 ;
        nowNumber = (nowNumber/2)*2 + 1;         
        // TODO 光剑摇摆嗡嗡音效   
        myDFPlayer.play(1);                
        humTimer = millis() - HUM_TIMEOUT + swng_time[nowNumber];
        swing_flag = 0;
        swing_timer = millis();
      }
      else if(GYR <= SWING_THR && GYR >  SWING_L_THR)
      {
        nowNumber = esp_random() % 15 ;
        nowNumber = (nowNumber/2)*2;         
        // TODO 光剑嗡嗡音效   
        myDFPlayer.play(1);                
        humTimer = millis() - HUM_TIMEOUT + swng_time[nowNumber];
        swing_flag = 0;
        swing_timer = millis();          
      }
    }
  }
}


 //随机脉冲
void randomPULSE() {
  //PULSE_ALLOW 使能脉冲 ls_state 按键打开
  if (PULSE_ALLOW && ls_state && (millis() - PULSE_timer > PULSE_DELAY) && lighton_flag == 0 && lightoff_flag == 0 && hit_flag == 0) {
    if (DEBUG) Serial.println("随机脉冲");
    PULSE_timer = millis();
    PULSEOffset = PULSEOffset * k + map((esp_random()%1024),0,1024,-PULSE_AMPL,PULSE_AMPL) * (1 - k);     //一阶滤波
    if (nowColor == 0) PULSEOffset = constrain(PULSEOffset, -15, 5);
    redOffset = constrain(red + PULSEOffset, 0, 255);
    greenOffset = constrain(green + PULSEOffset, 0, 255);
    blueOffset = constrain(blue + PULSEOffset, 0, 255);
    setAll(redOffset, greenOffset, blueOffset);
  }
}

//灯带刷新
void setAll(byte red, byte green, byte blue) {
  for (int i = 0; i < LED_COUNT; i++ ) {
    strip.setPixelColor(i, red, green, blue);
  }
  strip.show();
}
 //灯带开机动作
void create_lighton_action()
{
    lighton_flag = 1;
    light_counter = 0;
}
//灯带关机动作
void create_lightoff_action()
{
    lightoff_flag = 1;
    //light_counter =  LED_COUNT / 2 - 1; 
    light_counter =  LED_COUNT; 
}

//灯带敲击动作
void create_hit_action()
{
    setAll(255, 201, 0);        
    hit_timer = millis();
    hit_flag = 1;
}
 
//控制灯
void lightTick()
{  
   //ON
   if ((millis() - FLASH_timer > FLASH_DELAY) && lighton_flag )
   {  
      if (DEBUG) Serial.println("开灯");
      FLASH_timer = millis();
      strip.setPixelColor(light_counter, red, green, blue);
      strip.setPixelColor((LED_COUNT - 1 - light_counter), red, green, blue);
      strip.show();
      light_counter ++ ;
      if(light_counter >= LED_COUNT)
      {
        light_counter = 0;
        lighton_flag = 0;
      }
   }
   //OFF
   if ((millis() - FLASH_timer > FLASH_DELAY) && lightoff_flag)
   {     
      if (DEBUG) Serial.println("关灯");
      FLASH_timer = millis();
      strip.setPixelColor(light_counter, 0, 0, 0);
      strip.setPixelColor((LED_COUNT - 1 - light_counter), 0, 0, 0);
      strip.show();
      if(light_counter == 0)
      {
        light_counter = 0;
        lightoff_flag = 0;
      }
      light_counter -- ;
   }
   //HIT   
   if((millis() - hit_timer > FLASH_DELAY) && hit_flag && ls_state)
   {  
      if (DEBUG) Serial.println("敲击灯效");
      hit_timer = millis();
      setAll(red, blue, green);    
      hit_flag = 0;     
   }
}


//配置颜色
void setColor(byte color) {
  switch (color) {
    // 0 - red, 1 - green, 2 - blue, 3 - pink, 4 - yellow, 5 - ice blue
    case 0:
      red = 255;
      green = 0;
      blue = 0;
      break;
    case 1:
      red = 0;
      green = 0;
      blue = 255;
      break;
    case 2:
      red = 0;
      green = 255;
      blue = 0;
      break;
    // case 3:
    //   red = 255;
    //   green = 0;
    //   blue = 255;
    //   break;
    // case 4:
    //   red = 255;
    //   green = 255;
    //   blue = 0;
    //   break;
    // case 5:
    //   red = 0;
    //   green = 255;
    //   blue = 255;
      break;
  }
}
//电池检测
void batteryTick() {
  if (millis() - battery_timer > 30000 && ls_state && BATTERY_SAFE) {
    if (voltage_measure() < 15) {
      ls_chg_state = 1;
    }
    battery_timer = millis();
  }
}

//获取电池电量
byte voltage_measure() {
  voltage = 0;  
  for (int i = 0; i < 10; i++) {    
    voltage += (float)analogRead(VOLT_PIN) * 3.3 / 4095.0 ;
  }

  voltage = voltage / 10;          
  int volts = voltage * (R1 + R2)/R2  * 100;   
  if (volts > 387)
    return map(volts, 420, 387, 100, 77);
  else if ((volts <= 387) && (volts > 375) )
    return map(volts, 387, 375, 77, 54);
  else if ((volts <= 375) && (volts > 368) )
    return map(volts, 375, 368, 54, 31);
  else if ((volts <= 368) && (volts > 340) )
    return map(volts, 368, 340, 31, 8);
  else if (volts <= 340)
    return map(volts, 340, 260, 8, 0);
}