#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);

OneButton button_test(BTN_TEST, 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); // 发送状态

// 收
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();

void testbtn_click();

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;

// 需要做的初始化工作
enum Initstatus
{
  IS_Start, // 0. 刚上电
  IS_Wait_LengthZero,
  IS_LengthZero, // 1.已确定零点
  IS_Wait_WeightZero,
  IS_WeightZero,    // 2.已确定称重传感器0点
  IS_InStoreLocked, // 3. 挂钩入仓顶锁定
  IS_OK,            // 4.所有初始化已经OK，可以正常操作
  IS_Error          // 5.初始化遇到错误
} initstatus;

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);    // 抬起

  button_test.attachClick(testbtn_click);

  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();
  initstatus = IS_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);
}
// 初始化过程--
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
//
void checkinited()
{
  switch (initstatus)
  {
  case Initstatus::IS_Start:
  {
    // 开始自动慢速上升，直到顶部按钮按下
    printf("IS_Start: startup wait locked\n");
    motocontrol.setspeed(SPEED_BTN_SLOW);
    motocontrol.moto_run(MotoStatus::MS_Up);
    initstatus = IS_Wait_LengthZero;
    break;
  }
  case Initstatus::IS_Wait_LengthZero:
  {
    if (motocontrol.gethooktatus() == HS_Locked)
    {
      printf("IS_Wait_LengthZero: is locked\n");
      initstatus = IS_LengthZero;
    }
    break;
  }

  case Initstatus::IS_LengthZero:
  {
    // 开始自动慢速下降2cm
    printf("IS_LengthZero: Down 2cm \n");

    motocontrol.setspeed(SPEED_INSTORE);
    motocontrol.moto_run(MotoStatus::MS_Down, 10);
    initstatus = IS_Wait_WeightZero;
    break;
  }
  case Initstatus::IS_Wait_WeightZero:
  {
    if (motocontrol.gethooktatus() == HS_Stop)
    {
      printf("IS_Wait_WeightZero: Down Stop start tare and up \n");
      scale.tare();
      motocontrol.weightalign(true);
      motocontrol.moto_run(MotoStatus::MS_Up);
      initstatus = IS_WeightZero;
    }
    break;
  }

  case Initstatus::IS_WeightZero:
  {
    if (motocontrol.gethooktatus() == HS_Locked)
    {
      printf("IS_WeightZero: Locked \n");
      initstatus = IS_InStoreLocked;
    }
    break;
  }
  case Initstatus::IS_InStoreLocked:
  {
    printf("IS_InStoreLocked: IS_OK \n");
    initstatus = IS_OK;
    break;
  }
  }
}
// 在核心1上执行，重要的延迟低的
void loop()
{
  tksendinfo.update();    // 定时发送信息任务
  sercomm.getcommand();   // 得到控制命令
  button_checktop.tick(); // 按钮
  button_down.tick();     // 按钮
  button_up.tick();       // 按钮
  button_test.tick();
  motocontrol.setweight(pullweight); // 告诉电机拉的重量
  motocontrol.update();              // 电机控制
  showledidel();                     // 显示LED灯光
  showinfo();                        // 显示一些调试用信息
  // 到顶后延迟关闭动力电和舵机
  if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY))
  {
    _bengstop = false;
    motocontrol.setlocked(true);
  }
  // 检测执行初始化工作
  checkinited();
  delay(1);
}
// 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters)
{
  // 初始化称重传感器
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(963.f); // 这是缩放值，根据砝码实测516.f
  scale.tare();           // 重置为0

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

  //超重--红色
  if (motocontrol.getcontrolstatus().is_overweight)
    led_show(255, 0, 0); // red
}

/////////////////////////////////////按钮处理
// 顶部按钮，检测是否到顶部
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_BTN_SLOW);
    motocontrol.moto_run(MotoStatus::MS_Down);
  }
}
// 放线按钮--双击
void downbtn_dbclick()
{
  Serial.println("downbtn_dbclick");

  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_BTN_MID);
    motocontrol.moto_run(MotoStatus::MS_Down);
  }
}
// 放线按钮--长按
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();
}

// 收线按钮-单击
void upbtn_click()
{
  Serial.println("upbtn_click");
  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_BTN_SLOW);
    motocontrol.moto_run(MotoStatus::MS_Up);
  }
}
// 收线按钮-双击
void upbtn_dbclick()
{
  Serial.println("upbtn_dbclick");
  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stop();
  }
  else
  {
    motocontrol.setspeed(SPEED_BTN_MID);
    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 testbtn_click()
{
  Serial.println("testbtn_click");
  motocontrol.moto_goodsdown();
}