#include "Arduino.h"
#include "motocontrol.h"
#include "moto.h"

#define DEBUG_OUT false
Motocontrol::Motocontrol()
{
    _controlstatus.is_instore = false;
    _controlstatus.is_setzero = false;
    _controlstatus.zero_cnt = 0;
    _controlstatus.motostatus = MotoStatus::MS_Stop;
    _controlstatus.speed_targe = SPEED_SLOW;
    _check_targetlength = false;
}
Motocontrol::~Motocontrol()
{
}

bool Motocontrol::init() // 初始化
{
    return _moto_dji.init();
}
void Motocontrol::setspeed(float motospeed) // 设置速度
{
    _controlstatus.speed_targe = motospeed;
}
void Motocontrol::down(float length) // 放线
{
    printf("down:%.2f \n", length);
    moto_run(MotoStatus::MS_Down, length);
}
void Motocontrol::up(float length) // 收线
{
    printf("up:%.2f \n", length);
    moto_run(MotoStatus::MS_Up, length);
}
void Motocontrol::stop() // 停止
{
    _moto_dji.settarget(0.0f);
    _controlstatus.motostatus = MS_Stop;
    printf("stop \n");
}
void Motocontrol::setlocked(bool locked)
{
  if (locked)
  {
    stop();
    setzero();
  }
  _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::checkstatus() // 检查状态，比如什么时候停
{
    moto_measure_t mf = _moto_dji.getmotoinfo();
    // 已设置零点
    if (_controlstatus.is_setzero)
    {
        // 总放线长度
        float curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER;
        // 不能超ROPE_MAXLENGTH
        if ((curr_length > ROPE_MAXLENGTH)&&(_controlstatus.motostatus == MS_Down))
            stop();
        // 开始入仓
        if ((curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
        {
            _moto_dji.settarget(-SPEED_MIN);
            _controlstatus.is_instore = true;
        }
        else
            _controlstatus.is_instore = false;
    }
    if (_check_targetlength) // 有目标长度
    {
        if (_controlstatus.motostatus == MS_Down)
        {
            if (mf.output_round_cnt > _target_cnt)
            {
                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
                stop();
            }
        }
        else if (_controlstatus.motostatus == MS_Up)
        {
            if (mf.output_round_cnt < _target_cnt)
            {
                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
                stop();
            }
        }
    }
}

void Motocontrol::update() // 更新
{
    _moto_dji.update();
    checkstatus();
}
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
{
    _moto_dji.settarget(motospeed);
}

moto_measure_t Motocontrol::getmotoinfo()
{
    return _moto_dji.getmotoinfo();
}
control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
{
    return _controlstatus;
}
// 按指定速度收放线 放线为+，收线为- 单位cm
void Motocontrol::moto_run(MotoStatus updown, float length)
{
    // 传入要stop，直接停
    if (updown == MS_Stop)
    {
        stop();
        return;
    }

    // 运动中暂时不管
    if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
        return;
    // 没设置速度不转
    if (_controlstatus.speed_targe == 0)
        return;

    _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_SLOW;
        runspeed = -_controlstatus.speed_targe;
    }
    // 开始转
    printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
    _moto_dji.settarget(runspeed);
}