/// 电机控制业务逻辑封装-依赖moto类
/// @file motocontrol.cpp


#include "Arduino.h"
#include "motocontrol.h"
#include "moto.h"
#include <ESP32Servo.h>
#include "config.h"
#include "commser.h"

#define DEBUG_OUT false
static const char *MOUDLENAME = "MOTO_C";

Motocontrol::Motocontrol()
{
    _controlstatus.is_instore = false;
    _controlstatus.is_setzero = false;
    _controlstatus.is_overweight = false;
    _controlstatus.zero_cnt = 0;
    _controlstatus.motostatus = MotoStatus::MS_Stop;
    _controlstatus.speed_targe = SPEED_BTN_SLOW;
    _check_targetlength = false;
    _pullweight = 0;
    _curr_length = 0.0;
    _hooksstatus = HS_UnInit;
    _weightalign = false;
    _controlstatus.is_autogoodsdown = false;
    _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
    _unblocktimes = 0;
}
Motocontrol::~Motocontrol()
{
}

bool Motocontrol::init(Servo *lockservo) // 初始化
{
    _lockservo = lockservo;
    // 不管堵转问题，马上会执行其他初始化让舵机介绍
    _lockservo->write(SERVO_LOCKPOS);
    _servotatus = SS_ServoLocked;

    return _moto_dji.init();
}
void Motocontrol::setspeed(float motospeed, float acctime) // 设置速度
{
    _controlstatus.speed_targe = motospeed;
    _moto_dji.settime_acc(acctime);
}

void Motocontrol::setweight(int pullweight) // 设置重量
{
    // 到顶部锁定状态，有向上的压力，重量不准，直接退出
    if (_controlstatus.is_toplocked)
    {
        return;
    }
    _pullweight = pullweight;
    checkgoods();
}
String Motocontrol::gethooktatus_str(bool chstr)
{
    String hookstatusstr = "未知";
    String hookstatusstr_en = "unknown";
    switch (_hooksstatus)
    {
    case HS_UnInit:
        hookstatusstr = "未初始化";
        hookstatusstr_en = "HS_UnInit";
        break;
    case HS_Down:
        hookstatusstr = "放钩";
        hookstatusstr_en = "HS_Down";
        break;
    case HS_DownSlow:
        hookstatusstr = "慢速放钩";
        hookstatusstr_en = "HS_DownSlow";
        break;
    case HS_WaitUnhook:
        hookstatusstr = "等待脱钩";
        hookstatusstr_en = "HS_WaitUnhook";
        break;
    case HS_Up:
        hookstatusstr = "回收";
        hookstatusstr_en = "HS_Up";
        break;
    case HS_InStore:
        hookstatusstr = "入仓中";
        hookstatusstr_en = "HS_InStore";
        break;
    case HS_Locked:
        hookstatusstr = "已锁定";
        hookstatusstr_en = "HS_Locked";
        break;
    case HS_Stop:
        hookstatusstr = "停止";
        hookstatusstr_en = "HS_Stop";
        break;
    default:
        hookstatusstr = "未知";
        hookstatusstr_en = "HS_UnInit";
        break;
    }
    if (chstr)
        return hookstatusstr;
    else
        return hookstatusstr_en;
}

void Motocontrol::checkgoods() // 检测是否超重
{
    static unsigned long lastChangeTime = 0; // 上次状态变化的时间
    static unsigned long lastChangeTimeOverweight = 0; // 上次超重状态变化的时间
    const unsigned long debounceTime = 100;  // 防抖时间，单位：毫秒
   
    // 检查是否超重
    bool isOverweight = _pullweight > HOOK_WEIHT_MAX; // 当前是否超重
    if (isOverweight != _controlstatus.is_overweight) {
        // 如果状态持续超过防抖时间，则切换状态
        if (millis() - lastChangeTimeOverweight > debounceTime) {
            _controlstatus.is_overweight = isOverweight;
            lastChangeTimeOverweight = millis(); // 更新状态变化时间

            // 添加日志信息
            if (isOverweight) {
                addLogMessage("Overweight: " + String(_pullweight));
            } else {
                addLogMessage("Not overweight: " + String(_pullweight));
            }
        }
    } else {
        // 状态未变化，重置时间戳
        lastChangeTimeOverweight = millis();
    }


    // 检查是否有货物
    bool currentState = _pullweight >= HOOK_WEIHT_MIN; // 当前是否有货物
    if (currentState != _controlstatus.is_havegoods) {
        // 如果状态持续超过防抖时间，则切换状态
        if (millis() - lastChangeTime > debounceTime) {
            _controlstatus.is_havegoods = currentState;
            lastChangeTime = millis(); // 更新状态变化时间

            // 添加日志信息
            if (currentState) {
                addLogMessage("Have goods: " + String(_pullweight));
            } else {
                addLogMessage("No goods: " + String(_pullweight));
            }
        }
    } else {
        // 状态未变化，重置时间戳
        lastChangeTime = millis();
    }
}
void Motocontrol::lockservo() // 锁定舵机
{
    // ESP_LOGD(MOUDLENAME, "start_lockservo");
    addLogMessage("start_lockservo");
    _servotatus = SS_WaitMotoStop;
    _tm_servotatus = millis();
}
void Motocontrol::unlockservo() // 解锁舵机
{
    // ESP_LOGD(MOUDLENAME, "unlockservo");
    addLogMessage("unlockservo");
    // 解锁操作
    _lockservo->write(SERVO_UNLOCKPOS);
    _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
    _servotatus = SS_WaitUnLock;
    _tm_servotatus = millis();
    _unblocktimes = 0;
}
void Motocontrol::stoprun(float acctime) // 停止
{
    // ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime);
    addLogMessage("stoprun after time: " + String(acctime, 2));
    _moto_dji.settime_acc(acctime);
    _moto_dji.setspeedtarget(0.0f);
    _controlstatus.motostatus = MS_Stop;
    if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) && (_hooksstatus != HS_Locked))
    {
        _hooksstatus_prv = _hooksstatus;
        sethooksstatus(HS_Stop);
    }
    lockservo();
}
void Motocontrol::stopautodown()
{
    _controlstatus.is_autogoodsdown = false;
}
void Motocontrol::sethooksstatus(HookStatus hookstatus)
{
    _hooksstatus = hookstatus;
    // ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
    addLogMessage("Set HS: " + String(gethooktatus_str(false)));
}
void Motocontrol::setlocked(bool locked)
{
    if (locked)
    {
        stoprun();
        setzero();
        sethooksstatus(HS_Locked);
        _controlstatus.is_autogoodsdown = false;
    }
    _controlstatus.is_toplocked = locked;
}
void Motocontrol::setzero() // 设置0长度位置
{
    // 记录0长度位置--以后需要保存到falsh里面
    _controlstatus.zero_cnt = getmotoinfo().output_round_cnt;
    _controlstatus.is_setzero = true;
}
int16_t Motocontrol::getlength() // 得到长度
{
    return 0;
}

// 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign)
{
    _weightalign = weightalign;
}
void Motocontrol::checkhookstatus() // 检查挂钩状态
{
    // 需要在自动放货物中
    if (!_controlstatus.is_autogoodsdown)
        return;
    moto_measure_t mf = _moto_dji.getmotoinfo();
    _hook_currlen = _curr_length;
    switch (_hooksstatus)
    {
    // 正在下放货物
    case HookStatus::HS_Down:
    {
        // 如果没有货物--开始回收
        if (!_controlstatus.is_havegoods)
        {
            // ESP_LOGD(MOUDLENAME, "Not goods wait %d ms for unhook", HOOK_UNHOOK_TIME);
            addLogMessage("Not goods wait " + String(HOOK_UNHOOK_TIME) + " ms for unhook");
            sethooksstatus(HS_WaitUnhook);
            _tm_waitunhook = millis();
            break;
        }
        if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
        {
            // ESP_LOGD(MOUDLENAME, "HS_Down target is arrived curr: %.2f", mf.output_round_cnt);
            addLogMessage("HS_Down target is arrived curr: " + String(mf.output_round_cnt, 2));
            sethooksstatus(HS_DownSlow);
            _moto_dji.settime_acc(500);
            set_hook_speed(SPEED_HOOK_SLOW);
        }
        break;
    }
    // 开始慢速下放
    case HookStatus::HS_DownSlow:
    {
        if (!_controlstatus.is_havegoods)
        {
            // ESP_LOGD(MOUDLENAME, "Not havegoods wait %d ms unhook", HOOK_UNHOOK_TIME);
            addLogMessage("Not havegoods wait " + String(HOOK_UNHOOK_TIME) + " ms unhook");
            sethooksstatus(HS_WaitUnhook);
            _tm_waitunhook = millis();
        }
        break;
    }
    // 等待脱钩
    case HookStatus::HS_WaitUnhook:
    {
        if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
        {
            // ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup");
            addLogMessage("HS_WaitUnhook ok startup");
            _moto_dji.settime_acc(1000);
            set_hook_speed(-SPEED_HOOK_UP);
            sethooksstatus(HS_Up);
            _controlstatus.motostatus = MS_Up;
        }
        break;
    }
    // 开始收回
    case HookStatus::HS_Up:
    {
        // 会自动设置HS_Up状态
        break;
    }
    case HookStatus::HS_InStore:
    {
        // 会自动设置HS_InStore状态
        // 如果到顶部，会触发setlocked,不用在这里设置
        break;
    }
    }
}
// 检测舵机堵转---电流大或者角度不够，需要传感器
bool Motocontrol::checkservoblock()
{
    return false;
}
void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候停
{
    moto_measure_t mf = _moto_dji.getmotoinfo();

    // 停止状态---1防止舵机堵转，2延时关闭电机供电
    switch (_servotatus)
    {
    case SS_WaitUnLock: // 舵机解锁
    {
        if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
        {
            // 入仓时会自动设置速度，原速度不可用
            if (!_controlstatus.is_instore)
                _moto_dji.setspeedtarget(_runspeed);
            _servotatus = SS_ServoUnLocked;
        }
        break;
    }

        // 等待电机停止
    case SS_WaitMotoStop:
    {
        if (abs(mf.speed_rpm) < 3)
        {
            _lockservo->write(SERVO_LOCKPOS);
            _servotatus = SS_WaitServo;
            _tm_servotatus = millis();
        }
        break;
    }
    // 等待舵机执行到位---------防止堵转，功能没有使用，采用硬件防堵转舵机
    case SS_WaitServo:
    {
        // 舵机执行时间
        if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
        {
            // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
            if (!checkservoblock() || (_unblocktimes > 2))
            {
                // ESP_LOGD(MOUDLENAME, "Close moto");
                addLogMessage("Close moto");
                _moto_dji.close();
                _servotatus = SS_ServoLocked;
            }
            else
            // 堵转
            {
                // ESP_LOGD(MOUDLENAME, "SS  servoblock unlock servo and turn moto");
                addLogMessage("SS servoblock unlock servo and turn moto");
                _tm_servotatus = millis();
                _servotatus = SS_WaitUnBlock;
                // 写一个不会堵转的位置
                _lockservo->write(SERVO_BLOCKUNLOCKPOS);
                // 转一点电机角度
                _moto_dji.setspeedtarget(SPEED_UNBLOCK);
            }
        }

        break;
    }
    case SS_WaitUnBlock:
    {
        if ((millis() - _tm_servotatus) > TM_UNBLOCK)
        {
            // ESP_LOGD(MOUDLENAME, "SS lock servo");
            addLogMessage("SS lock servo");
            // 继续锁定等待舵机检测
            _lockservo->write(SERVO_LOCKPOS);
            _servotatus = SS_WaitServo;
            _tm_servotatus = millis();
            _unblocktimes++;
        }
        break;
    }
    }

    // 已设置零点
    if (_controlstatus.is_setzero)
    {
        // 总放线长度
        _curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER;
        // 不能超ROPE_MAXLENGTH
        if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
            stoprun();
        // 开始入仓
        // if ((_curr_length < (INSTORE_LENGTH_MIN + (mf.output_speed_rpm * mf.output_speed_rpm) * INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up))
        float instlen;
        if (_controlstatus.is_havegoods)
            instlen = INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
        else
            instlen = INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
        if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up))
        {
            if (!_controlstatus.is_instore)
            {
                sethooksstatus(HS_InStore);
                _moto_dji.settime_acc(500);
                set_hook_speed(-SPEED_INSTORE);
                _controlstatus.is_instore = true;
                // ESP_LOGD(MOUDLENAME, "Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
                addLogMessage("Instorelen: " + String(instlen, 2) + ", speed: " + String(mf.output_speed_rpm, 2));
            }
        }
        else
        {
            // 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
            if (_hooksstatus != HS_InStore)
                _controlstatus.is_instore = false;
        }
    }
    if (_check_targetlength) // 有目标长度
    {
        if (_controlstatus.motostatus == MS_Down)
        {
            if (mf.output_round_cnt > _target_cnt)
            {
                // ESP_LOGD(MOUDLENAME, "stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
                addLogMessage("stop down tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt));
                stoprun(1000); // 缓慢停止
            }
        }
        else if (_controlstatus.motostatus == MS_Up)
        {
            if (mf.output_round_cnt < _target_cnt)
            {
                // ESP_LOGD(MOUDLENAME, "stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
                addLogMessage("stop up tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt));
                stoprun(1000); // 缓慢停止
            }
        }
    }
}

void Motocontrol::set_hook_speed(float hooksspeed)
{
    _goods_speed = hooksspeed;
    _runspeed = hooksspeed;
    _moto_dji.setspeedtarget(hooksspeed);
}
void Motocontrol::update() // 更新
{
    _moto_dji.update();
    checkmotostatus();
    checkhookstatus();
}
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
{
    _moto_dji.setspeedtarget(motospeed);
}

moto_measure_t Motocontrol::getmotoinfo()
{
    return _moto_dji.getmotoinfo();
}
control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
{
    return _controlstatus;
}

void Motocontrol::moto_goodsdownresume()
{
    if (!_controlstatus.is_autogoodsdown)
        return;
    if (_hooksstatus == HS_Stop)
    {
        _runspeed = _goods_speed;
        if (_hooksstatus_prv == HS_Up)
            _controlstatus.motostatus = MS_Up;
        if (_hooksstatus_prv == HS_InStore)
        {
            _controlstatus.motostatus = MS_Up;
            _runspeed = -SPEED_INSTORE;
        }
        sethooksstatus(_hooksstatus_prv);
        _moto_dji.settime_acc(1000);
        unlockservo(); // 打开舵机
        // _moto_dji.setspeedtarget(_goods_speed);
    }
}
// 长度cm
void Motocontrol::moto_goodsdown(float length)
{
    if (length < 0.0)
    {
        // ESP_LOGD(MOUDLENAME, "length<0 fault");
        addLogMessage("length<0 fault");
        return;
    }
    // 没设置零点
    if (!_controlstatus.is_setzero)
    {
        // ESP_LOGD(MOUDLENAME, "not lengthzero fault");
        addLogMessage("not lengthzero fault");
        return;
    }
    if (!_weightalign)
    {
        // ESP_LOGD(MOUDLENAME, "weight not align fault");
        addLogMessage("weight not align fault");
        return;
    }
    // 没挂东西
    if (!_controlstatus.is_havegoods)
    {
        // ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
        addLogMessage("goods min fault: " + String(_pullweight));
        return;
    }

    // 开始自动放货物状态
    _controlstatus.is_autogoodsdown = true;
    // 长度=0，直接中慢速下降，
    if (length == 0.0)

    {
        _goods_down_target_cnt = 0;
        setspeed(SPEED_HOOK_CHECK);
        _goods_speed = SPEED_HOOK_CHECK;
    }
    else
    {
        float tarleng = length;
        if (length > HOOK_SLOWDOWN_LENGTH)
            tarleng -= HOOK_SLOWDOWN_LENGTH;
        _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
        _goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
        // ESP_LOGD(MOUDLENAME, "start down %.2f cm,tar:%.2f", tarleng, _goods_down_target_cnt);
        addLogMessage("start down " + String(tarleng, 2) + " cm, tar: " + String(_goods_down_target_cnt, 2));
        setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
        _goods_speed = SPEED_HOOK_FAST;
    }
    moto_run(MS_Down);
}

// 按指定速度收放线 放线为+，收线为- 单位cm
void Motocontrol::moto_run(MotoStatus updown, float length)
{
    // 传入要stop，直接停
    if (updown == MS_Stop)
    {
        stoprun();
        return;
    }

    // 运动中暂时不管
    if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
    {
        // ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up");
        addLogMessage("motostatus is MS_Down\\MS_Up");
        return;
    }
    // 没设置速度不转
    if (_controlstatus.speed_targe == 0)
    {
        // ESP_LOGD(MOUDLENAME, "not set speed_targe");
        addLogMessage("not set speed_targe");
        return;
    }
    if (updown == MS_Up)
    {
        if (_controlstatus.is_toplocked)
        {
            // ESP_LOGD(MOUDLENAME, "is_toplocked return");
            addLogMessage("is_toplocked return");
            return;
        }
        if (_controlstatus.is_overweight)
        {
            // ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
            addLogMessage("overweight fault :" + String(_pullweight));
            return;
        }
        sethooksstatus(HS_Up);
    }
    else
        sethooksstatus(HS_Down);

    unlockservo();

    _controlstatus.motostatus = updown;
    // 有长度限制
    if (length != 0)
    {
        // 收需要负
        if (updown == MotoStatus::MS_Up)
            length = -length;
        // 记录开始位置
        _start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
        _target_cnt = _start_cnt + length / WHEEL_PERIMETER;
        _check_targetlength = true;
    }
    else
        _check_targetlength = false;

    // 记录开始位置
    float runspeed;
    if (updown == MotoStatus::MS_Down)
        runspeed = _controlstatus.speed_targe;
    else
    {
        // 如果没有设置零点，不能快速收线
        if (!_controlstatus.is_setzero)
            _controlstatus.speed_targe = SPEED_BTN_SLOW;
        runspeed = -_controlstatus.speed_targe;
    }
    // 开始转
    // ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
    _runspeed = runspeed;
    // _moto_dji.setspeedtarget(runspeed);
}