#include "witmotiondll.h"

//当没有注册 延时函数（m_delayFunction）时，运行会停止在调用延时函数（m_delayFunction）处；
//此函数就是为了解决上面说的问题
void delay_tc(uint32_t millisecond)
{
    ;
}

void printf_tc(const char* text)
{
    ;
}

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

    m_delayFunction = delay_tc;

    m_witPrintf = printf_tc;
}

int WitmotionDll::delayMsRegister(delay delayFunction)
{
    m_delayFunction = delayFunction;

    return 0;
}

int WitmotionDll::printfRegister(witPrintf printfFunction)
{
    m_witPrintf = printfFunction;

    return 0;
}

void WitmotionDll::setDelayTimeMs(uint32_t millisecond)
{
    TIME_TO_SLEEP = millisecond;
}

int WitmotionDll::constructAndSendInstruction(int registerAddress, int registerContent)
{
    uint8_t unBuffer[8];
    unBuffer[0]=0xFF;
    unBuffer[1]=0xAA;
    unBuffer[2]=registerAddress & 0xFF;
    unBuffer[3]=registerContent & 0xff;
    unBuffer[4]=registerContent >> 8;

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

    return num;
}

int WitmotionDll::unlockInstruction()
{
    int numOfSend = constructAndSendInstruction(KEY, KEY_UNLOCK);
    m_witPrintf("WitmotionDll::unlockInstruction: \n");

    return 0;
}

int WitmotionDll::saveInstruction()
{
    int numOfSend = constructAndSendInstruction(SAVE, SAVE_PARAM);
    m_witPrintf("WitmotionDll::saveInstruction: \n");

    return 0;
}

int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(AXIS6, algorithm);
    m_witPrintf("WitmotionDll::algorithm: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(ORIENT, orient);
    m_witPrintf("WitmotionDll::installationOrientation:\n");

    saveInstruction();
    return 0;
}

int WitmotionDll::instructStart(POWONSEND_ENUM command)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(POWONSEND, command);
    m_witPrintf("WitmotionDll::instructStart: \n");

    saveInstruction();
    return 0;
}

int WitmotionDll::exitCalibration()
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(CALSW, EXITCAL);
    m_witPrintf("WitmotionDll::exitCalibration: \n");

    saveInstruction();
    return 0;
}

int WitmotionDll::magneticCalibration()
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(CALSW, CALMAG);
    m_witPrintf("WitmotionDll::magneticCalibration: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setHeightToZero()
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(CALSW, CALALTITUDE);
    m_witPrintf("WitmotionDll::setHeightToZero: \n");

    saveInstruction();

    return 0;
}

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

    int numOfSend = constructAndSendInstruction(CALSW, CALANGLEZ);
    m_witPrintf("WitmotionDll::setZAxisAngleToZero: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setAngleReference()
{
    unlockInstruction();

    uint16_t registerContent = 0x08;//询问技术支持才得到的命令
    int numOfSend = constructAndSendInstruction(CALSW, registerContent);
    m_witPrintf("WitmotionDll::setAngleReference: \n");

    saveInstruction();

    return 0;
}

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

int WitmotionDll::setTimeZone(TIMEZONE_ENUM timeZone)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(TIMEZONE, timeZone);
    m_witPrintf("WitmotionDll::setTimeZone: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setContent(RETURN_CONTENT_STRUCT content)
{
    unlockInstruction();

    uint16_t registerContent = 0;

    if(content.time)
        SetBit(registerContent, TIME);
    if(content.acceleration)
        SetBit(registerContent, ACCELERATION);
    if(content.angular_velocity)
        SetBit(registerContent, ANGULAR_VELOCITY);
    if(content.euler_angle)
        SetBit(registerContent, EULER_ANGLE);
    if(content.magnetic_field)
        SetBit(registerContent, MAGNETIC_FIELD);
    if(content.port_status)
        SetBit(registerContent, PORT_STATUS);
    if(content.atmospheric_pressure_altitude)
        SetBit(registerContent, ATMOSPHERIC_PRESSURE_ALTITUDE);
    if(content.latitude_longitude)
        SetBit(registerContent, LATITUDE_LONGITUDE);

    if(content.ground_velocity)
        SetBit(registerContent, GROUND_VELOCITY);
    if(content.quaternion)
        SetBit(registerContent, QUATERNION);
    if(content.satellite_accuracy)
        SetBit(registerContent, SATELLITE_ACCURACY);

    int numOfSend = constructAndSendInstruction(RSW, registerContent);
    m_witPrintf("WitmotionDll::setContent: \n");

    saveInstruction();
    return 0;
}

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

int WitmotionDll::SetBaudrate(BAUD_ENUM baudrate)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(BAUD, baudrate);
    m_witPrintf("WitmotionDll::SetBaudrate: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(RRATE, returnRate);
    m_witPrintf("WitmotionDll::SetReturnRate: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::SetDeviceAddress(int deviceAddress)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(IICADDR, deviceAddress);
    m_witPrintf("WitmotionDll::SetDeviceAddress: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D0MODE, model);
    m_witPrintf("WitmotionDll::setD0Model: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D1MODE, model);
    m_witPrintf("WitmotionDll::setD1Model: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D2MODE, model);
    m_witPrintf("WitmotionDll::setD2Model: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D3MODE, model);
    m_witPrintf("WitmotionDll::setD3Model: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D0PWMH, PWMH);
    m_witPrintf("WitmotionDll::setD0HighLevelPulseWidth: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D1PWMH, PWMH);
    m_witPrintf("WitmotionDll::setD1HighLevelPulseWidth: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D2PWMH, PWMH);
    m_witPrintf("WitmotionDll::setD2HighLevelPulseWidth: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D3PWMH, PWMH);
    m_witPrintf("WitmotionDll::setD3HighLevelPulseWidth: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD0Period(int period)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D0PWMT, period);
    m_witPrintf("WitmotionDll::setD0Period: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD1Period(int period)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D1PWMT, period);
    m_witPrintf("WitmotionDll::setD1Period: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD2Period(int period)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D2PWMT, period);
    m_witPrintf("WitmotionDll::setD2Period: \n");

    saveInstruction();

    return 0;
}

int WitmotionDll::setD3Period(int period)
{
    unlockInstruction();

    int numOfSend = constructAndSendInstruction(D3PWMT, period);
    m_witPrintf("WitmotionDll::setD3Period: \n");

    saveInstruction();

    return 0;
}