#include "witmotiondll.h"

WitmotionDll::WitmotionDll(SerialPortBase * serialPort)
{
    m_SerialPort = serialPort;
}

int WitmotionDll::unlockInstruction()
{
    using std::this_thread::sleep_for;

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=KEY & 0xFF;
    unBuffer[3]=KEY_UNLOCK & 0xff;
    unBuffer[4]=KEY_UNLOCK >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    std::cout<<"WitmotionDll::unlockInstruction: "<< num <<std::endl;

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    return 0;
}

int WitmotionDll::saveInstruction()
{
    using std::this_thread::sleep_for;

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=SAVE & 0xFF;
    unBuffer[3]=SAVE_PARAM & 0xff;
    unBuffer[4]=SAVE_PARAM >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    std::cout<<"WitmotionDll::saveInstruction: "<< num <<std::endl;

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    return 0;
}

int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;

    switch (algorithm)
    {
    case ALGROITHM6:
    {
        unBuffer[2]=AXIS6 & 0xFF;
        unBuffer[3]=ALGROITHM6 & 0xff;
        unBuffer[4]=ALGROITHM6 >> 8;

        break;
    }
    case ALGROITHM9:
    {
        unBuffer[2]=AXIS6 & 0xFF;
        unBuffer[3]=ALGROITHM9 & 0xff;
        unBuffer[4]=ALGROITHM9 >> 8;

        break;
    }
    default:
        break;
    }

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=ORIENT & 0xFF;
    unBuffer[3]=orient & 0xff;
    unBuffer[4]=orient >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();
    return 0;
}

int WitmotionDll::instructStart(POWONSEND_ENUM command)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=POWONSEND & 0xFF;
    unBuffer[3]=command & 0xff;
    unBuffer[4]=command >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();
    return 0;
}

int WitmotionDll::exitCalibration()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=CALSW & 0xFF;
    unBuffer[3]=EXITCAL & 0xff;
    unBuffer[4]=EXITCAL >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();
    return 0;
}

int WitmotionDll::magneticCalibration()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=CALSW & 0xFF;
    unBuffer[3]=CALMAG & 0xff;
    unBuffer[4]=CALMAG >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setHeightToZero()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=CALSW & 0xFF;
    unBuffer[3]=CALALTITUDE & 0xff;
    unBuffer[4]=CALALTITUDE >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setZAxisAngleToZero()
{
    //需要读取参数，确保处于算法ALGROITHM6下



    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=CALSW & 0xFF;
    unBuffer[3]=CALANGLEZ & 0xff;
    unBuffer[4]=CALANGLEZ >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setAngleReference()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=CALSW & 0xFF;
    unBuffer[3]=0x08;
    unBuffer[4]=0x00;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

void WitmotionDll::recordData()
{
    m_SerialPort->ReadData();
}

int WitmotionDll::setTimeZone(TIMEZONE_ENUM timeZone)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=TIMEZONE & 0xFF;
    unBuffer[3]=timeZone & 0xff;
    unBuffer[4]=timeZone >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();
    return 0;
}

int WitmotionDll::latitudeLongitude()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=RSW & 0xFF;

    //设置回传内容
    uint8_t RSWL = 0;
    uint8_t RSWH = 0;

    SetBit(RSWL, LATITUDE_LONGITUDE);

    unBuffer[3] = RSWL;
    unBuffer[4] = RSWH;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    exitCalibration();
    return 0;
}

int WitmotionDll::eulerAngle()
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=RSW & 0xFF;

    //设置回传内容
    uint8_t RSWL = 0;
    uint8_t RSWH = 0;

    SetBit(RSWL, EULER_ANGLE);

    unBuffer[3] = RSWL;
    unBuffer[4] = RSWH;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    exitCalibration();
    return 0;
}

int WitmotionDll::setContent(RETURN_CONTENT_ENUM content)
{
    return 0;
}

int WitmotionDll::clearContent(RETURN_CONTENT_ENUM content)
{
    return 0;
}

int WitmotionDll::SetBaudrate(BAUD_ENUM baudrate)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=BAUD & 0xFF;
    unBuffer[3]=baudrate & 0xff;
    unBuffer[4]=baudrate >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=RRATE & 0xFF;
    unBuffer[3]=returnRate & 0xff;
    unBuffer[4]=returnRate >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::SetDeviceAddress(int deviceAddress)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=IICADDR & 0xFF;
    unBuffer[3]=deviceAddress & 0xff;
    unBuffer[4]=deviceAddress >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D0MODE & 0xFF;
    unBuffer[3]=model & 0xff;
    unBuffer[4]=model >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D1MODE & 0xFF;
    unBuffer[3]=model & 0xff;
    unBuffer[4]=model >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D2MODE & 0xFF;
    unBuffer[3]=model & 0xff;
    unBuffer[4]=model >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D3MODE & 0xFF;
    unBuffer[3]=model & 0xff;
    unBuffer[4]=model >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D0PWMH & 0xFF;
    unBuffer[3]=PWMH & 0xff;
    unBuffer[4]=PWMH >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D1PWMH & 0xFF;
    unBuffer[3]=PWMH & 0xff;
    unBuffer[4]=PWMH >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D2PWMH & 0xFF;
    unBuffer[3]=PWMH & 0xff;
    unBuffer[4]=PWMH >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D3PWMH & 0xFF;
    unBuffer[3]=PWMH & 0xff;
    unBuffer[4]=PWMH >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0Period(int period)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D0PWMT & 0xFF;
    unBuffer[3]=period & 0xff;
    unBuffer[4]=period >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1Period(int period)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D1PWMT & 0xFF;
    unBuffer[3]=period & 0xff;
    unBuffer[4]=period >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2Period(int period)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D2PWMT & 0xFF;
    unBuffer[3]=period & 0xff;
    unBuffer[4]=period >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3Period(int period)
{
    using std::this_thread::sleep_for;

    unlockInstruction();

    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=D3PWMT & 0xFF;
    unBuffer[3]=period & 0xff;
    unBuffer[4]=period >> 8;

    int num = m_SerialPort->SendData1((char *)unBuffer,5);

    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));

    saveInstruction();

    return 0;
}