#include "IS11Comon.h"
u_char BufferForRead[10000];
int TotalIndexNow = 0;
u_char BufferFortempWrite[1000];
// MySerialWrite=nullptr;
SERIALWRITE MySerialWrite = nullptr;
SERIALREAD MySerialRead = nullptr;
bool ISIS11Init = false;
uint16_t crc16(const uint8_t *data, size_t len, uint16_t polynomial)
{
    uint16_t i, j, tmp, CRC16;

    CRC16 = 0xFFFF; // CRC寄存器初始值
    for (i = 0; i < len; i++)
    {
        CRC16 ^= data[i];
        for (j = 0; j < 8; j++)
        {
            tmp = (uint16_t)(CRC16 & 0x0001);
            CRC16 >>= 1;
            if (tmp == 1)
            {
                CRC16 ^= polynomial; // 异或多项式
            }
        }
    }
    return CRC16;
}

size_t SendSettingCommand(u_char *Command, size_t CommandLenth, u_char *Value, size_t ValueLenth)
{
    memcpy(BufferFortempWrite, Command, CommandLenth);
    memcpy(BufferFortempWrite + CommandLenth, Value, ValueLenth);
    uint16_t crc = crc16(BufferFortempWrite, CommandLenth + ValueLenth);
    memcpy(BufferFortempWrite + CommandLenth + ValueLenth, &crc, 2);
    
    SerialWrite(BufferFortempWrite, CommandLenth + ValueLenth + 2);

    return GetSetBackFromSensor();
}

size_t SendGetData(int shutter)
{
    int lenth = sizeof(GET_ALL_DN);
    uint16_t crc = crc16(GET_ALL_DN, lenth);
    memcpy(BufferFortempWrite, GET_ALL_DN, lenth);
    memcpy(BufferFortempWrite + lenth, &crc, 2);
    SerialWrite(BufferFortempWrite, lenth + 2);

    vTaskDelay(shutter);

    return GetInfoBackFromSensor(true);
}

size_t SendGetSensorInfo(u_char *Command, size_t lenth)
{

    uint16_t crc = crc16(Command, lenth);
    memcpy(BufferFortempWrite, Command, lenth);
    memcpy(BufferFortempWrite + lenth, &crc, 2);

    SerialWrite(BufferFortempWrite, lenth + 2);

    String datastr;
    for (size_t i = 0; i < lenth + 2; i++)
    {
        datastr += String(BufferFortempWrite[i]);
    }
    // write_log(gp_log, "send data:" + datastr, 20);

    size_t retunnumber = GetInfoBackFromSensor();

    return retunnumber;
}

size_t GetSetBackFromSensor()
{
    TotalIndexNow = 0;
    TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);

    while (TotalIndexNow < 8)
    {
        TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);

    }
    return TotalIndexNow;
}

bool panduanHeader()
{
    if (TotalIndexNow < 2)
    {
        return false;
        /* code */
    }
    int temp = 0;
    while (BufferForRead[temp] != 0x01 &&
           !(BufferForRead[temp + 1] == 0x03 || BufferForRead[temp + 1] == 0x06 || BufferForRead[temp + 1] == 0x10))
    {
        if (temp >= TotalIndexNow - 2)
        {
            break;
            /* code */
        }
        temp++;

        /* code */
    }
    memcpy(BufferForRead, BufferForRead + temp, TotalIndexNow - temp);
    TotalIndexNow = TotalIndexNow - temp;
    temp = 0;
    if (BufferForRead[temp] != 0x01 &&
        !(BufferForRead[temp + 1] == 0x03 || BufferForRead[temp + 1] == 0x06 || BufferForRead[temp + 1] == 0x10))
    {
        return false;
    }
    return true;
}

size_t GetInfoBackFromSensor(bool isbig) // big 是指用几个字节表示数据长度 暂时只发现采集数据时用两个字节
{

    if (isbig)
    {   

        TotalIndexNow = 0;
        memset(BufferForRead, 0, sizeof(BufferForRead));
        TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 2);
        while (!panduanHeader())
        {
            TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);
        }
        while (TotalIndexNow < 4)
        {
            TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);
        }

        int lenth = BufferForRead[2] * 256 + BufferForRead[3];
        while (TotalIndexNow < lenth + 4)
        {
            TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1024);
        }
        return TotalIndexNow;
    }
    else
    {
        // 长度用一个字节表示
        TotalIndexNow = 0;
        TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);

        while (TotalIndexNow < 3)
        {
            TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);
        }
        int lenth = BufferForRead[2];
        while (TotalIndexNow < lenth + 5)
        {
            TotalIndexNow += SerailRead(BufferForRead + TotalIndexNow, 1);
        }
        return TotalIndexNow;
    }
}

size_t SerialWrite(u_char *data, size_t lenth)
{
    if (MySerialWrite != nullptr)
    {
        return MySerialWrite(data, lenth);
    }


    return 0;
}

size_t SerailRead(u_char *data, size_t lenth)
{
    if (MySerialRead != nullptr)
    {
        return MySerialRead(data, lenth);
    }
    return 0;
}

void InitFunction(SERIALWRITE a, SERIALREAD readfunc)
{
    MySerialWrite = a;
    MySerialRead = readfunc;
    ISIS11Init = true;
}

u_char *GetDataBufferPTR()
{
    return BufferForRead;
}
bool isSensorInit()
{
    return ISIS11Init;
}

void CoverLittleAndBig(char *data, int lenth)
{
    char *tempdata = new char[lenth];
    memcpy(tempdata, data, lenth);

    for (size_t i = 0; i < lenth / 2; i++)
    {
        data[2 * i] = tempdata[2 * i + 1];
        data[2 * i + 1] = tempdata[2 * i];
        /* code */
    }
    delete[] tempdata;
}