#pragma once
#include "Arduino.h"
#include "config.h"
#include "moto.h"
#include <ESP32Servo.h>

#define ROPE_MAXLENGTH 700                               // 最多能放700cm---实际绳子应该比这个长750之类的
#define WHEEL_DIAMETER 3.8                               // 轮子直径cm
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416)        // 轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数

#define INSTORE_LENGTH_MIN_NONE 10.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_MIN_GOODS 8.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！

#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！

#define SPEED_MAX 350             // 最快收放线速度--任何时候不应该超过这个速度
#define SPEED_INSTORE 5           // 入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20         // 按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100         // 按键中等收放线速度
#define SPEED_BTN_FAST 200        // 按键最快收放线速度
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20        // 货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50       // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP SPEED_MAX   // 空钩上升速度
#define SPEED_UNBLOCK 0.1         // 舵机堵转需要缓慢转的速度

#define TM_UNBLOCK 100   // 舵机堵转需要缓慢转等待的时间ms
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500    // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些

#define SERVO_LOCKPOS 1920        // 舵机锁定位置
#define SERVO_UNLOCKPOS 1800      // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小

#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西  (克)

#define HOOK_WEIHT_MAX 6000 // 最大货物重量 大于这个认为超重不工作  (克)

#define HOOK_SLOWDOWN_LENGTH 10 // 下放物体时快到目标长度时慢速长度  (cm)
#define HOOK_UNHOOK_TIME 1500   // 等待脱钩时间ms

// 挂钩状态
enum MotoStatus
{
  MS_Down, // 自动下放中
  MS_Up,   // 回收中
  MS_Stop  // 停止中
};

typedef struct
{
  bool is_instore;       // 正在进仓
  bool is_setzero;       // 已设置0长度位置
  bool is_toplocked;     // 已到顶锁住
  bool is_overweight;    // 是否超重
  bool is_havegoods;     // 是否有货物
  float zero_cnt;        // 0长度位置
  float speed_targe;     // 当前目标速度
  MotoStatus motostatus; // 电机运行状态

} control_status_t;
// 锁定舵机状态
enum ServoStatus
{
  SS_WaitMotoStop, // 等待电机停止

  SS_ServoUnLocked, //

  SS_WaitUnLock,  //
  SS_WaitServo,   // 等待舵机不堵转
  SS_WaitUnBlock, // 等待舵机不堵转的位置--同时电机会缓慢旋转
  SS_ServoLocked  // 成功停止
};

class Motocontrol
{
private:
  Servo *_lockservo;
  moto _moto_dji;
  float _start_cnt;
  float _target_cnt;

  float _goods_down_start_cnt;
  float _goods_down_target_cnt;
  float _goods_speed;

  control_status_t _controlstatus;
  bool _check_targetlength;
  ServoStatus _servotatus;
  unsigned long _tm_servotatus;
  int _pullweight;
  HookStatus _hooksstatus;
  HookStatus _hooksstatus_prv;
  int _hook_currlen;
  float _curr_length;
  bool _weightalign;
  bool _autogoodsdown;
  uint8_t _overweightcount;

  uint8_t _notweightcount;
  uint8_t _unblocktimes;
  unsigned long _tm_waitunhook;
  float _runspeed;

  void checkmotostatus();
  void checkhookstatus();
  void goodsstartup();
  void checkgoods();
  bool checkservoblock();
  void lockservo();
  void unlockservo();
  void set_hook_speed(float hooksspeed);

public:
  Motocontrol();                                         // 构造函数
  ~Motocontrol();                                        // 析构函数
  void setspeed(float motospeed, float acctime = -1);    // 设置速度
  void stop(float acctime = 0);                          // 停止
  void setzero();                                        // 设置0长度位置
  int16_t getlength();                                   // 得到长度
  void update();                                         // 更新
  bool init(Servo *lockservo);                           // 初始化
  void direct_speed(float motospeed);                    // 直接设置电机速度--立马生效,仅用于测试
  moto_measure_t getmotoinfo();                          // 得到电机信息
  control_status_t getcontrolstatus();                   // 得到控制信息
  void setlocked(bool locked);                           // 是否到顶锁定
  void moto_run(MotoStatus updown, float length = 0.0f); // 收放线
  void setweight(int pullweight);                        // 设置货物实时重量
  void moto_goodsdown(float length = 0.0f);              // 放下货物,自动回收
  void moto_goodsdownresume();                           // 恢复自动放线
  HookStatus gethooktatus() { return _hooksstatus; }     // 得到挂钩状态
  String gethooktatus_str();    // 得到挂钩状态

  void weightalign(bool weightalign);                    // 重量传感器是否校准--必须校准才能执行moto_hookdown
};