#include <Arduino.h>
#include "HX711.h"
#include "OneButton.h"
#include "Serialcommand.h"
#include "config.h"
#include <Ticker.h> //调用Ticker.h库
#include <soc/rtc_cntl_reg.h>
#include "motocontrol.h"
#include "moto.h"
#include <FastLED.h>
#include <ESP32Servo.h>
#define TM_INSTORE_DELAY 100   //入仓延时关闭时间  ms

// 角度传感器
// 收放线电机控制
// 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚

//---------------------------------
OneButton button_up(BTN_UP, true);
OneButton button_down(BTN_DOWN, true);
OneButton button_checktop(BTN_CT, true);
HX711 scale;
Serialcommand sercomm;
Servo myservo; 
#define NUM_LEDS 1
#define DATA_PIN 25
CRGB leds[NUM_LEDS];
Motocontrol motocontrol;

void sendinfo();
Ticker tksendinfo(sendinfo, 500); // 发送状态

HookStatus hookstatus = HS_Unknow;

// 收
void upbtn_click();
void upbtn_dbclick();
void upbtn_pressstart();
void upbtn_pressend();
// 放
void downbtn_click();
void downbtn_dbclick();
void downbtn_pressstart();
void downbtn_pressend();
// 顶抬起
void ctbtn_pressend();
// 顶按下
void ctbtn_pressstart();

int get_pullweight();
void Task1(void *pvParameters);
void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters );
void showledidel();
int pullweight = 0;
 unsigned long _tm_bengstop;
 bool _bengstop=false;


void setup()
{
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
  // 启用Esp32双核第二个核心
  xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 1, NULL, 0); // 最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行.

  // 调试串口
  Serial.begin(115200);
  Serial.println("Starting PullupDevice...");

  // 初始化按钮
  button_up.attachClick(upbtn_click);
  button_up.attachDoubleClick(upbtn_dbclick);
  button_up.attachLongPressStart(upbtn_pressstart);
  button_up.attachLongPressStop(upbtn_pressend);

  button_down.attachDoubleClick(downbtn_dbclick);
  button_down.attachClick(downbtn_click);
  button_down.attachLongPressStart(downbtn_pressstart);
  button_down.attachLongPressStop(downbtn_pressend);

  button_checktop.setPressTicks(10);                      // 10毫秒就产生按下事件，用于顶部按钮检测
  button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件，
  button_checktop.attachLongPressStop(ctbtn_pressend);    // 抬起

  ESP32PWM::allocateTimer(0);
	ESP32PWM::allocateTimer(1);
	ESP32PWM::allocateTimer(2);
	ESP32PWM::allocateTimer(3);
	myservo.setPeriodHertz(50);    // standard 50 hz servo
	myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object


  // 初始化RGB  LED   显示黄色灯表示需要注意 LED
  FastLED.addLeds<WS2812, DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
  if (!motocontrol.init(&myservo)) // 初始化电机控制
    printf("motocontrol init fault\n");

  // 发送状态任务开启--500ms一次
  tksendinfo.start();

  // if (motocontrol.getstatus()==MS_Stop)
  // {
  //     //slowup();

  // }

  Serial.println("PullupDevice is ready!");
}
void slowup()
{
  // motocontrol.setspeed(SPEED_UP_SLOW);
  // motocontrol.up(0);  //一直收线直到到顶部
}
void showinfo()
{
  //  moto_measure_t mf=motocontrol.getmotoinfo() ;
  //  printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
  // if (pullweight>10)
  //  printf("PullWeight:%d\n",pullweight);
}

// 在核心1上执行，重要的延迟低的
void loop()
{
  tksendinfo.update();    // 定时发送信息任务
  sercomm.getcommand();   // 得到控制命令
  button_checktop.tick(); // 按钮
  button_down.tick();     // 按钮
  button_up.tick();       // 按钮
  motocontrol.update();   // 电机控制
  showledidel();          //显示LED灯光
  showinfo();             // 显示一些调试用信息
  if ((_bengstop)&&((millis()-_tm_bengstop)>TM_INSTORE_DELAY))
  {
    _bengstop=false;
    motocontrol.setlocked(true);
    hookstatus = HS_Lock;
  }

  delay(1);
}
// 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters)
{
  // 初始化称重传感器
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(516.f); // 这是缩放值，根据砝码实测
  scale.tare();           // 重置为0

  // 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
  while (1)
  {
    pullweight = get_pullweight();
    vTaskDelay(10);
  }
}
void sendinfo() // 每500ms发送状态信息
{
  sercomm.sendinfo(hookstatus, 5324, pullweight);
}

float Value;
#define FILTER_A 0.5
// 低通滤波和限幅后的拉力数值,单位:克
int get_pullweight()
{
  float NewValue;
  if (scale.wait_ready_timeout(100)) // 等待数据ok,100ms超时
    NewValue = scale.get_units();
  else
    NewValue = 0;
  Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
  Value = constrain(Value, 0.0, 6000.0);                  // 限制到0-6公斤
  return round(Value);
}
// 提示灯光控制
void led_show(uint8_t cr,uint8_t cg,uint8_t cb)
{
  leds[0] =CRGB(cg,cr,cb);//颜色交叉了
  FastLED.show();
}
// 显示颜色
void showledidel()
{
  switch (motocontrol.getcontrolstatus().motostatus)
  {
  case MotoStatus::MS_Down:
  {
    led_show(0,0,255);  //blue

    break;
  }
  case MotoStatus::MS_Up:
  {
    
    if (motocontrol.getcontrolstatus().is_instore)
      led_show(255,0,0); // red
    else
      led_show(200,0,50); // red
    break;
  }

  case MotoStatus::MS_Stop:
  {
    if (motocontrol.getcontrolstatus().is_toplocked)
    {
       led_show(0,255,0); //green
    }else
    {
      if (motocontrol.getcontrolstatus().is_setzero)
        led_show(255,255,255);           // 紫色
      else
        led_show(255,255,0);   //yellow
    }
    break;
  }
  }
}

/////////////////////////////////////按钮处理
// 顶部按钮，检测是否到顶部
void ctbtn_pressstart()
{
  Serial.println("ctbtn_pressstart");
  
  _tm_bengstop=millis();
  _bengstop=true;

}

void ctbtn_pressend()
{
  Serial.println("ctbtn_pressend");
  motocontrol.setlocked(false);
  _bengstop=false;
}
void downbtn_click()
{
  Serial.println("downbtn_click");

  if (motocontrol.getcontrolstatus().motostatus!= MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_SLOW);
    motocontrol.moto_run(MotoStatus::MS_Down);
   }
}
void upbtn_click()
{
  Serial.println("upbtn_click");
  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_SLOW);
    motocontrol.moto_run(MotoStatus::MS_Up);
  }
}

void downbtn_dbclick()
{
  Serial.println("downbtn_dbclick");

  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_FAST);
    motocontrol.moto_run(MotoStatus::MS_Down);
  }
}
void upbtn_dbclick()
{
  Serial.println("upbtn_dbclick");
  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_FAST);
    motocontrol.moto_run(MotoStatus::MS_Up);
  }
}

void upbtn_pressstart()
{
  Serial.println("upbtn_pressstart");
  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_BTN_FAST);
    motocontrol.moto_run(MotoStatus::MS_Up);
  }
}
void upbtn_pressend()
{
  Serial.println("upbtn_pressend");
  motocontrol.stop();
}

void downbtn_pressstart()
{
  Serial.println("downbtn_pressstart");
  if (motocontrol.getcontrolstatus().motostatus!= MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_BTN_FAST);
    motocontrol.moto_run(MotoStatus::MS_Down);
  }
}
void downbtn_pressend()
{
  Serial.println("downbtn_pressend");
  motocontrol.stop();
}