#include "motorboard.h"
#include <iostream>
#include "PinMonitor.h"
#include "uartregsaccess.h"
#include <iomanip>
#include "stringex.hpp"
#include "config.h"
#include "StopWatch.h"
#include "applog.h"
#include "StateControl.h"
#include "Displaydef.h"
#include "MotorConfig.h"
#include "DisplayCenter.h"
#include <bitset>
#include "SensorConfig.h"

static const std::string loggername = "MotorBoard";


MotorBoard::MotorBoard(std::shared_ptr<WakeUp> wake)
    : devPort(MOTOR_UART),
      m_glue({nullptr, nullptr, nullptr,nullptr,nullptr,nullptr,nullptr}),
      autopaperkeystop(nullptr),
      m_config({0})
{
    LOG_INIT();
    //m_uartEnable.reset(new GpioOut(149));
    //m_uartEnable->setValue(Gpio::Low);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    m_regsAccess.reset(new UartRegsAccess(devPort, bauds, 0x07, 0x87));
    m_intPinMonitor.reset(new PinMonitor(intport, std::bind(&MotorBoard::pin_call, this, std::placeholders::_1)));
    //m_uartEnable->setValue(Gpio::High);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    m_os_mode = os_mode();
    m_statecontrol.reset(new StateControl(m_regsAccess,wake));
    m_wake = wake;
    init_sensor_duty();
}

static int paperinnum = 0;
void MotorBoard::start(HGScanConfig cfg)
{
    m_config = cfg;
    keep_last_paper=false;
    paperinnum = 0;
    m_paperout_count = 0;
    clear_error();
    set_motor_stop(false);
    set_time_error(120);
    set_double_inpect(m_config.g200params.double_feed_enbale);
    set_staple_inpect(m_config.g200params.stable_enbale);
    set_paper_inspect(0);
    set_auto_paper(m_config.g200params.is_autopaper,m_config.g200params.en_anlogic_key);
    set_screw_inpect(m_config.g200params.screw_detect_enable);
    set_screw_level(m_config.g200params.screw_detect_level);
    set_long_paper(true);
    set_double_out_en(m_config.g200params.double_out_en);
    unsigned int val;
    SMBCONFIG *smbc = (SMBCONFIG *)(&val);
    read(0, val);
    smbc->enable = 0;
    write(0, val);
    smbc->enable = 1;
    write(0, val);

    en_lifter();
}

void MotorBoard::stop()
{
    printf("MotorBoard Stop \n");
    unsigned int val;
    SMBCONFIG *smbc = (SMBCONFIG *)(&val);
    read(0, val);
    smbc->enable = 0;
    write(0, val);
}
bool MotorBoard::en_lifter()
{
    unsigned int val;
    SMBCONFIG *smbc = (SMBCONFIG *)(&val);
    read(0x00, val);
    smbc->lifter_en = 1;
    write(0x00, val);
    smbc->lifter_en = 0;
    return write(0x00, val);
}

void MotorBoard::pick_paper(void)
{
    unsigned int val,pick_en;
    SMBCONFIG *smbc = (SMBCONFIG *)(&val);
    read(0x00, val);
    smbc->pick_paper = 0;
    write(0x00, val);
    std::this_thread::sleep_for(std::chrono::microseconds(400));
    smbc->pick_paper = 1;
    write(0x00, val);
    std::string v_str = std::to_string(m_version);
    if(v_str.size()<8)
        return;
    uint32_t date = atoi(v_str.substr(2,6).c_str());
#ifdef G100
    if(date < 231115)
        return;
#else
    if(date < 231122)
        return;
#endif
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    read(0x09,pick_en);
    if(!(pick_en&0xffff))
    {
        smbc->pick_paper = 0;
        write(0x00, val);
        std::this_thread::sleep_for(std::chrono::microseconds(400));
        smbc->pick_paper = 1;
        write(0x00, val);
    }
    // smbc->pick_paper = 0;
    // write(0x00, val);
}

void MotorBoard::clean_paper_road(){
    unsigned int val;
    SMB_FUNC *smbc = (SMB_FUNC *)(&val);
    read(6, val);
    if(smbc->param.work_mode == 0)
    {
        smbc->param.func_clean_passthro = 1;
        write(6, val);
        smbc->param.func_clean_passthro = 0;
        write(6, val);
    }
    else{
        printf("\n 非空闲模式不允许清理纸道！！！！");
    }
   
}

void MotorBoard::clear_error()
{
    unsigned int val;
    SMBCONFIG *smbc = (SMBCONFIG *)(&val);
    read(0, val);
    smbc->error_clean = 1;
    write(0, val);
    smbc->error_clean = 0;
    write(0, val);
}

bool MotorBoard::wait_arrival_top(int timeout_ms)
{
    return cv_arrival_top.wait(timeout_ms);
}

bool MotorBoard::wait_paper_in(int timeout_ms)
{
    return cv_paper_in.wait(timeout_ms);
}

bool MotorBoard::wait_error(int timeout_ms)
{
    return cv_error.wait(timeout_ms);
}

bool MotorBoard::wait_paper_out(int timeout_ms)
{
    // StopWatch sw;
    // LOG_TRACE("wait_paper_out ");
    // while(sw.elapsed_ms()<timeout_ms)
    // {
    //     if(m_paperout_count > 0)
    //     {
    //         m_paperout_count--;
    //         return true;
    //     } 
    //     std::this_thread::sleep_for(std::chrono::milliseconds(1));
    // }
    // return false;
    return cv_paper_out.wait(timeout_ms);
}

bool MotorBoard::wait_done(int timeout_ms)
{
    return cv_scan_done.wait(timeout_ms);
}

int MotorBoard::os_mode()
{
    // unsigned int val;
    // read(0x02, val);
    // SMB_MODE *smb_mode = (SMB_MODE *)&val;
    // return smb_mode->scan_mode;
    unsigned int val;
    read(0x06,val);
    SMB_FUNC  smb_func = *(SMB_FUNC*)&val;
    return smb_func.param.work_mode == 1;
}

bool MotorBoard::paper_ready()
{
    unsigned int val;
    read(0x02, val);
    SMB_MODE *smb_mode = (SMB_MODE *)&val;
    return smb_mode->feeding_paper_ready;
}

bool MotorBoard::is_scanning()
{
    unsigned int val;
    read(0x02, val);
    SMB_MODE *smb_mode = (SMB_MODE *)&val;
    return smb_mode->work_status;
}

int MotorBoard::paper_counter()
{
    unsigned int val;
    read(0x02, val);
    SMBMODE *smb_mode = (SMBMODE *)&val;
    return smb_mode->scan_num;
}

bool MotorBoard::set_paper_inspect_param(unsigned int value /* = 1000 */)
{
    unsigned int val;
    if (!read(0x04, val))
        return false;
    SMBCONFIGEXT *smb_config_ext = (SMBCONFIGEXT *)&val;
    smb_config_ext->error_range_set = value;
    return write(0x04, val);
}

bool MotorBoard::get_keeplastpaper(){
    return keep_last_paper;
}

bool MotorBoard::set_paper_inpect_info(unsigned int value)
{
    unsigned int val;
    if (!read(0x04, val))
        return false;
    SMBCONFIGEXT *smb_config_ext = (SMBCONFIGEXT *)&val;
    smb_config_ext->paper_infor = value;
    return write(0x04, val);
}

bool MotorBoard::set_paper_inspect(bool enable /* = true */)
{
    unsigned int val;
    if (!read(0x04, val))
        return false;
    SMBCONFIGEXT *smb_config_ext = (SMBCONFIGEXT *)&val;
    smb_config_ext->paper_size_check_en = enable;
    return write(0x04, val);
}

bool MotorBoard::set_double_inpect(bool enable)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    enable?m_statecontrol->lcdcontrol(4):m_statecontrol->lcdcontrol(5);
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->double_paper = enable;
    return write(0x00, val);
}
bool MotorBoard::get_doublle_inpect()
{
    return 0;
}
bool MotorBoard::set_double_out_en(bool enable)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->double_out_en = enable;
    return write(0x00, val);
}
bool MotorBoard::get_double_out_en()
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    return smb_config->double_out_en;
}
bool MotorBoard::set_staple_inpect(bool enable)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->staple_enable = enable;
    return write(0x00, val);
}
bool MotorBoard::get_staple_inpect()
{
	return 0;
}

bool MotorBoard::set_cuospeed(int value)
{
    unsigned int val;
    if (!read(0x4, val))
        return false;
    SMBCONFIGEXT *smb_config = (SMBCONFIGEXT *)&val;
    smb_config->cuo_speed = value;
    return write(0x04, val);
}

// bool MotorBoard::set_en600DPI(bool en)
// {
//     unsigned int val;
//     if (!read(0x00, val))
//         return false;
//     SMBCONFIG *smb_config = (SMBCONFIG *)&val;
//     smb_config->dpi600 = en?1:0;
//     return write(0x00, val);
// }
bool MotorBoard::set_color_mode(int mode)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->color_mode = mode;
    return write(0x00, val);
}

bool MotorBoard::set_slowmoire(bool en){
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->slow_moire = en;
    return write(0x00, val);
}


int MotorBoard::get_color_mode()
{
    return 0;
}

bool MotorBoard::set_speed_mode(int mode)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->speed_set_enable = 1;
    smb_config->v_setting = mode;
    write(0x00, val);
    smb_config->speed_set_enable = 0;
    return    write(0x00, val);
}

bool MotorBoard::set_speed_mode_v_temp(int mode)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    printf(" set v_tmp = %d \n",mode);
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->speed_set_enable = 1;
    smb_config->v_temp = mode;
    write(0x00, val);
    smb_config->speed_set_enable = 0;
    return    write(0x00, val);
}

int MotorBoard::get_speed_mode()
{
    unsigned int val;
    if (!read(0x00, val))
        return -1;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    return smb_config->v_setting;
}

std::shared_ptr<IRegsAccess> MotorBoard::regs()
{
    return m_regsAccess;
}
static int countindex =0;
void MotorBoard::pin_call(unsigned int pinNum)
{
    static int index = 0;
    // int os_m = os_mode();  //安路屏蔽计数 扫描过程中无法操作按键
    // if (m_os_mode != os_m) 
    // {
    //     m_os_mode = os_m;
    //     cv_os_mode.notify_all();
    //     if (m_glue.m_os_mode_call)
    //         m_glue.m_os_mode_call(m_os_mode);
    // }

    // if (m_os_mode)  //安路屏蔽计数返回 以刷新计数状态
    // {
    //     LOG_TRACE("not scan mode");
    //     return;
    // }

    unsigned int val;
    SMBSTATUS *smb_status = (SMBSTATUS *)&val;
    if (!read(0x01, val))
        LOG_TRACE("read error");
    LOG_TRACE(string_format("status %08x", val));
    if(val & 0x800){
         //printf("\n keep_last_paper ");
         keep_last_paper=true;
    }
    if(val & 0x1000)
    {
        if(m_glue.m_set_sleepmode_call)
            m_glue.m_set_sleepmode_call(val & 0xf000);
    }
    if(val & 0x80000)
    {
        if(m_glue.m_mltop_call)
            m_glue.m_mltop_call(val);
    }
    if(smb_status->auto_feed)
    {
        if(m_glue.m_auto_paper)
            m_glue.m_auto_paper(1);
    }
    if (val & 0x7c003FE)
    {
		SetKeyState(false);
        cv_error.notify_all();
        if (m_glue.m_error_call)
            m_glue.m_error_call(val & 0x30efe); //0xefe index of 16:aquireimage error index of bit 17 :size check error 
        if(val & 0x30efe){
            cv_paper_out.notify_all();
            cv_paper_in.notify_all();
            m_paperout_count++;
        }
		errormsg(val & 0x1c003fa);
		if((val & 0x4) ||(val & 0x02000000))
        {   
            if(m_glue.m_coveropen_call)
               m_glue.m_coveropen_call((val & 0x4));//cover open & 0x04
            if(val & 0x4){
                PutMsg(DisType::Dis_Err_CoverOpen,0,ClearScreen::All);
                set_auto_paper(false,false);
                autopaperkeystop?autopaperkeystop():void(0);
                if(m_statecontrol){
                    m_statecontrol->setcoverstate(true);
                    m_statecontrol->setmenuindex(0);
                }
            }
            else{
                PutMsg(DisType::Dis_Idel,0,ClearScreen::All);
                m_statecontrol?m_statecontrol->setcoverstate(false):void();
            }
            return;
        }
        if(smb_status->double_clean_f)
            PutMsg(DisType::Dis_Idel,0,ClearScreen::All);
        LOG_TRACE("error");
        return;
    }
    else
    {
        if (!smb_status->scan_pulse)
        {
            cv_paper_in.notify_all();
            unsigned int papercount = 0;
            read(0x02,papercount);
            SMBMODE smbmode = *(SMBMODE*)&papercount;
            printf("paper in arm count = %d  ,motorcount = %d\n",++countindex,smbmode.scan_num);
            startcapimage(true);
            PutMsg(DisType::Dis_Scan_Page, smbmode.scan_num,ClearScreen::BOT);
        }
        if(smb_status->paper_left)
        {
            cv_paper_out.notify_all();
            m_paperout_count++;
            //printf("paper out  time = %s \n",GetCurrentTimeStamp(2).c_str());
            startcapimage(true);
            LOG_TRACE(string_format("m_paperout_count %s",to_string(m_paperout_count)));
        }
    }

    if (val & 0x400)
    {
        LOG_TRACE("done");
        cv_scan_done.notify_all();
        if (m_glue.m_scan_done_call)
            m_glue.m_scan_done_call();
        cv_paper_out.notify_all();
        cv_paper_in.notify_all();
        clear_error();
        SetKeyState(false);
        if(m_wake.get())
            m_wake->setsleepfalg(false);
    }
}

bool MotorBoard::write(unsigned int addr, unsigned int val)
{
    return m_regsAccess.get()?m_regsAccess->write(addr, val):false;
}

bool MotorBoard::read(unsigned int addr, unsigned int &val)
{
    return m_regsAccess.get()?m_regsAccess->read(addr, val):false;
}

 bool MotorBoard::set_time_error(int value){
    unsigned int val;
    if (!read(0x05, val))
        return false;
    SMBCONFIGTIME *smb_config = (SMBCONFIGTIME *)&val;
    smb_config->error_time_set = value;
    return write(0x05, val);
 }

void MotorBoard::set_callbacks(MotorBoardGlue glue)
{
    m_glue = glue;
}

bool MotorBoard::set_screw_inpect(bool enable)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->skew_enable = enable;
    return write(0x00, val);
}
bool MotorBoard::get_screw_inpect()
{
    unsigned int val;
    read(0x00, val);
    SMBCONFIG *smb_mode = (SMBCONFIG *)&val;
    return smb_mode->skew_enable;
}

bool MotorBoard::set_screw_level(int level)
{
        unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->skew_parameter = level;
    return write(0x00, val);
}

bool MotorBoard::set_auto_paper(bool enable,bool enkey){
    unsigned int val;
    if (!read(0x00, val))
        return false;
    m_statecontrol?m_statecontrol->setautopaperflag(enable,enkey):void(0);
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->autofeed_mode = enable;
    return write(0x00, val);
}

bool MotorBoard::set_long_paper(bool enable)
{
    unsigned int val;
    if (!read(0x00, val))
        return false;
    SMBCONFIG *smb_config = (SMBCONFIG *)&val;
    smb_config->paper = enable;
    return write(0x00, val);
}

int MotorBoard::get_screw_level()
{
    unsigned int val;
    auto ret= read(0x00, val);
    if(!ret)
        return -1;
    SMBCONFIG *smb_mode = (SMBCONFIG *)&val;
    return smb_mode->skew_parameter;
}

void MotorBoard::start_countmode()
{
    unsigned int regval=0;

    read(0x06,regval);
    LOG_TRACE(string_format("func6 regval = %08x",regval));
    SMBFUNC func = *(SMBFUNC*)&regval;
    func.param.work_mode =1;
    func.param.func_feed_mid = 1;
    func.param.func_clear_count = 1;
    LOG_TRACE(string_format("func6 value = %08x",func.value));
     write(0x06,func.value);
    func.param.func_encount = 1;
    func.param.key_sound = 1;
    func.param.func_clear_count = 0;
    LOG_TRACE(string_format("func6 value = %08x",func.value));
    write(0x06,func.value);
    func.param.func_encount = 0;
    func.param.key_sound = 0;
    LOG_TRACE(string_format("func6 value = %08x",func.value));
    write(0x06,func.value);
}


void MotorBoard::PutMsg(DisType type,int value,ClearScreen clearscreen)
{
    if(m_statecontrol.get())
        m_statecontrol->PutMsg(type,value,clearscreen);
}

void MotorBoard::errormsg(uint value)
{
    if(value & 0x4)
        PutMsg(DisType::Dis_Err_CoverOpen,0,ClearScreen::All);
    else if (value & 0x2)
        PutMsg(DisType::Dis_Err_NoPaper,0,ClearScreen::All);
    else if (value & 0x8)
        PutMsg(DisType::Dis_Err_FeedError,0,ClearScreen::All);
    // else if (value & 0x10)
    //     PutMsg(DisType::Dis_Err_JamIn,0,ClearScreen::All);
    else if (value & 0x20)
        PutMsg(DisType::Dis_Err_DoubleFeed,0,ClearScreen::All);
    else if (value & 0x40)
        PutMsg(DisType::Dis_Err_Stable,0,ClearScreen::All);
    else if (value & 0x80)
        PutMsg(DisType::Dis_Err_PaperScrew,0,ClearScreen::All);
    else if (value & 0x00010000)
        PutMsg(DisType::Dis_Err_AqrImgTimeout,0,ClearScreen::All);
    else if((value & 0x1000010) == 0x1000010)
        PutMsg(DisType::Dis_Err_JamIn,3,ClearScreen::All);
    else if((value & 0x800010) == 0x800010)
        PutMsg(DisType::Dis_Err_JamIn,2,ClearScreen::All);
    else if((value & 0x400010) == 0x400010)
        PutMsg(DisType::Dis_Err_JamIn,1,ClearScreen::All);
}

void MotorBoard::SetKeyState(bool value)
{
    if(m_statecontrol)
        m_statecontrol->setrunstate(value);
}


void MotorBoard::set_keystopenable(bool value){
    unsigned int regval=0;
    read(0x06,regval);
    LOG_TRACE(string_format("func6 regval = %08x",regval));
    SMBFUNC func = *(SMBFUNC*)&regval;
    func.param.key_stop_enable = value;
    write(0x06,regval);
}


void MotorBoard::set_freq(int motor_choose,int speedmode,int colormode,int dpi)
{
    MotorConfig cf;
    auto params = cf.GetMotorSpeedParams(motor_choose == 1?0:1,MotorConfig::MTBDType::MT_DRV);
    MotorSpeedParam param;
    //printf("\n---node.dpi ==%d && node.colormode == %d  && node.speed == %d----- ",dpi,colormode,speedmode);
    for(auto &node : params)
    {
        if(node.dpi == dpi && node.colormode == colormode && node.speed == speedmode)
        {
            param = node.mt_param;
            //printf("\n-------------------------------------------------");
            break;
        }
    }
    std::vector<std::uint32_t> table;
    if(motor_choose == 0)
    {
        int x = (dpi == 1?(Get_static_jsonconfig().getscannerinfo().chu_motor_speed_200):(dpi == 2?Get_static_jsonconfig().getscannerinfo().chu_motor_speed_300 : Get_static_jsonconfig().getscannerinfo().chu_motor_speed_600));
        unsigned int regval=0;
        read(0x06,regval);
        SMBFUNC func = *(SMBFUNC*)&regval;
        func.param.motor_choose = motor_choose;
        func.param.wr_en = 1;
        write(0x06,func.value);
        write(0x04,x);
        //func.param.wr_en = 0;
        //write(0x06,func.value);
        return ;
    }
    if(motor_choose == 1)
        table = frep_cfg(param.finalPeriod,param.Fmin,param.stepnum,param.a,param.offset,param.finalDelay,param.acceleration_time);
    if(motor_choose == 2)
        table = frep_cfg(param.finalPeriod,param.Fmin,param.stepnum,param.a,param.offset,param.finalDelay,param.acceleration_time);
    if(motor_choose == 3)
        table = frep_cfg(param.finalPeriod,param.Fmin,param.stepnum,param.a,param.offset,param.finalDelay,param.acceleration_time);
    unsigned int regval=0;
    read(0x06,regval);
    SMBFUNC func = *(SMBFUNC*)&regval;
    int start_addr_cuo = 0;
#ifdef G200
    if(motor_choose ==1 && speedmode > 1 && dpi <3)
        start_addr_cuo = 63;
    if(motor_choose ==1 &&  dpi ==3)
        start_addr_cuo = 127;
#endif
    printf("\nstart_addr_cuo   =%d ",start_addr_cuo);
    for(int i =0;i<256;i++)
    {
        func.param.motor_choose = motor_choose;
        func.param.wr_en = 1;
        func.param.motor_addr =i;
        write(0x06,func.value);
        write(0x04,i<start_addr_cuo?0:table[i-start_addr_cuo]);
        //printf("\nfreq= %x addr =%d ",table[i-start_addr_cuo],i);
    }
    func.param.wr_en = 0;
    write(0x06,func.value);
}

void MotorBoard::init_statecontrol()
{
    m_regsAccess.reset(new UartRegsAccess(devPort, bauds, 0x07, 0x87));
    m_intPinMonitor.reset(new PinMonitor(intport, std::bind(&MotorBoard::pin_call, this, std::placeholders::_1)));
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    m_os_mode = os_mode();
    m_statecontrol.reset(new StateControl(m_regsAccess,m_wake));
    m_statecontrol->setautopaperstopcallback([&]{
        autopaperkeystop?autopaperkeystop():void(0);
        if(m_config.g200params.is_autopaper)
            set_auto_paper(false,false);
    });
    init_sensor_duty();
    PutMsg(DisType::Dis_Idel,0,ClearScreen::All);
}

void MotorBoard::release_statecontrol()
{
    
    if(autopaperkeystop.operator bool())
        autopaperkeystop();
    set_auto_paper(false,false);
    m_statecontrol.reset();
    m_regsAccess.reset();
    m_intPinMonitor.reset();

}

void MotorBoard::setautopaperkeystopcallback(std::function<void()> func){
    if(func)
        autopaperkeystop = func;
    m_statecontrol?m_statecontrol->setautopaperstopcallback([&]{
        autopaperkeystop?autopaperkeystop():void(0);
        set_auto_paper(false,false);
    }):void(0);
}
void MotorBoard::startcapimage(bool value)
{
    // if(m_config.g200params.is_fixedpaper)
    //     return;
	// FILE *fp = fopen("/sys/class/tty/ttyUSB0/device/huagao_scanner", "w");
	// if (fp == NULL)
	// 	perror("startcapimage open filed");
	// else{
	// 	fprintf(fp, "%d", value ? 1 : 0);
	// 	fclose(fp);
	// }
}

bool MotorBoard::set_sensor_pwm_duty(int sensorid,int duty)
{
    //1：扫描；2：开盖；3：歪斜—1；4：歪斜-2；5：出纸口；6：有无纸
    printf("set_sensor_pwm_duty type = %d  duty = %d \n",sensorid,duty);
    unsigned int val;
    if (!read(0x05, val))
        return false;
    if(sensorid< 1 || sensorid >6)
        return false;
    if(duty< 2 || duty > 90)
        return false;
    SMBCONFIGTIME *smb_config_time = (SMBCONFIGTIME *)&val;
    smb_config_time->sen_duty_set_sel = sensorid;
    smb_config_time->duty_set = duty*8+50;
    write(0x05, val);
    smb_config_time->sen_duty_set_sel = 0;
    return write(0x05, val);
}

bool MotorBoard::enable_sensor_pwm(int sensorid,bool en)
{
    std::uint32_t val;
    if (!read(0x05, val))
        return false;
    if(sensorid< 1 || sensorid >6)
        return false;
    std::bitset<32> bit(val);
    bit.set(sensorid+6,en);
    val = bit.to_ulong();
    printf("enable_sensor_pwm = %d \n",val);
    return write(0x05, val);
}

bool MotorBoard::set_ultrasonic_param(int type,int value){
    //1：双张检测周期；2：有无纸检测周期；3：双张阈值；4：单张阈值；
    unsigned int val;
    if (!read(0x07, val))
        return false;
    if(type< 1 || type >4)
        return false;
    printf("set_ultrasonic_param type = %d  value = %d \n",type,value);
    SMB_Ultrasonic_Config *tmp = (SMB_Ultrasonic_Config *)&val;
    switch (type)
    {
    case 1:
        tmp->param.double_check_cyc = value;
        break;
    case 2:
        tmp->param.paper_check_cyc = value;
        break;
    case 3:
        tmp->param.double_max = value;
        break;
    case 4:
        tmp->param.single_max = value;
        break;
    default:
        break;
    }
    printf("val = %d \n",val);
    tmp->param.send_cyc_en = 1;
    write(0x07,val);
    tmp->param.send_cyc_en = 0;
    return write(0x07,val);
}

void MotorBoard::init_sensor_duty()
{
    auto p = Get_Static_SC().get_mem_param();
    std::vector<uint32_t> params(sizeof(SensorConfig::SensorConfig_Param)/sizeof(uint32_t));
    memcpy(&params[0],&p,sizeof(SensorConfig::SensorConfig_Param));
    for(int i = 0;i<params.size();i++){
        if(((i >= 0) || (i < 6)) && (params[i] < 100))
        {
            set_sensor_pwm_duty(i+1,params[i]);
        }
        if((i >= 6) || (i < 10))
        {
            set_ultrasonic_param(i-5,params[i]);
        }
    }
}

std::uint32_t MotorBoard::get_ultrasonic_version(){
    unsigned int val;
    if (!read(0x07, val))
        return 0;
    SMB_Ultrasonic_Config *tmp = (SMB_Ultrasonic_Config *)&val;
    tmp->param.rd_ver_en = 1;
    write(0x07,val);
    tmp->param.rd_ver_en = 0;
    write(0x07,val);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    read(0x08,val);
    return val;
}

std::string MotorBoard::getmbversion(){
    u32 value = 0;
	read(0x03, value);
    m_version = value;
	printf("mb version: = %s \n", std::to_string(value).c_str());
	return std::to_string(value);
}

bool MotorBoard::set_motor_stop(bool value)
{
    SMB_FUNC reg6;
    if(!read(6,reg6.value))
        return false;
    reg6.param.motor_disable = value;
    return write(6,reg6.value);
}

SMB_JAM_Time MotorBoard::get_jam_param()
{
    SMB_JAM_Time jam_time{0};
    read(0xa,jam_time.value);
    return jam_time;
}

void MotorBoard::set_jam_param(SMB_JAM_Time jam_time)
{
    write(0xa,jam_time.value);
}