#include "IS3Comon.h"
#ifdef _WIN32
#include <windows.h>
void delay(int ms){
    Sleep(ms);
}
#else
#include <unistd.h> // For usleep

void delay(int ms){
    usleep(ms * 1000); // Convert milliseconds to microseconds
}
#include <unistd.h>
#include <time.h>
#ifndef _POSIX_C_SOURCE
    #define _POSIX_C_SOURCE 199309L
#endif
#endif
#include <cstring>
#include <Communication_Protocol.h>

unsigned long long get_system_uptime_ms() {
#ifdef _WIN32
    return GetTickCount64();

#else
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return (unsigned long long)(ts.tv_sec) * 1000 + ts.tv_nsec / 1000000;


#endif
}







u_char BufferForRead[10000];
int TotalIndexNow = 0;
u_char BufferFortempWrite[1000];
// MySerialWrite=nullptr;
SERIALWRITE MySerialWrite = nullptr;
SERIALREAD MySerialRead = nullptr;
bool isIS3Init = 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)
{

    int lenthforwrite = IRIS_Protocol_Pack(Command, (uint16_t)ValueLenth, Value, (uint8_t *)BufferFortempWrite);
    SerialWrite(BufferFortempWrite, lenthforwrite);
    // for 5 次
    for (int i = 0; i < 10; i++)
    {
        size_t retunnumber = GetInfoBackFromSensorinTime(Command,1000);
        if (retunnumber!=OVERTIME ) {
            return retunnumber;
        }
        if (i%2==0) {
            printf("send command again\n");
            SerialWrite(BufferFortempWrite, lenthforwrite);

        }

        // 第I次尝试 数据回复异常 数据确实 用英文打印
        printf("warning  try %d times: date not enough,try again!!\n",i);


    }

    printf("ERROR date not enough,retunr !!\n");
    return OVERTIME;


}

size_t SendGetData(int shutter)
{
    uint8_t dataforsend=NONE_DATA;
    int lenthforwrite=IRIS_Protocol_Pack(GET_DATA_FROM_SENSOR, (uint16_t)01, &dataforsend, (uint8_t *)BufferFortempWrite    );
    SerialWrite(BufferFortempWrite, lenthforwrite);
    for (int i=0;i<10;i++)
    {
        size_t retunnumber = GetInfoBackFromSensorinTime(GET_DATA_FROM_SENSOR,2*shutter+2000);
        if (retunnumber<OVERTIME) {
            return retunnumber;
        }
        if (i%5==0) {
            printf("send command again\n");
            SerialWrite(BufferFortempWrite, lenthforwrite);

        }
        // 第I次尝试 数据回复异常 数据确实 用英文打印
        printf("warning  try %d times: date not enough,try again!!\n",i);
    }

    // size_t retunnumber = GetInfoBackFromSensor(GET_DATA_FROM_SENSOR);


    return OVERTIME;
}

size_t SendGetSensorInfo(u_char Command, size_t lenth)
{
    uint8_t dataforsend=NONE_DATA;

   int lenthforwrite= IRIS_Protocol_Pack(Command,(uint16_t)01,&dataforsend,(uint8_t *)BufferFortempWrite);


       //delay(200);
    SerialWrite(BufferFortempWrite, lenthforwrite);

    size_t retunnumber = GetInfoBackFromSensor(Command);

    return retunnumber;
}

size_t GetSetBackFromSensor()
{
    return 0;
}



size_t GetInfoBackFromSensor(uint8_t Command) // big 是指用几个字节表示数据长度 暂时只发现采集数据时用两个字节
{
    delay(1);
    TotalIndexNow = 0;
    BufferForRead[0] = 0x00;
    memset(BufferForRead, 0, 200);


    size_t retrunlenth=SerailRead(BufferForRead + TotalIndexNow, 2);


    TotalIndexNow += retrunlenth;
    // TotalIndexNow=IRIS_Cut_Befor_Header(BufferForRead,TotalIndexNow);
    while (TotalIndexNow<5) {
        size_t retrunlenth=SerailRead(BufferForRead + TotalIndexNow, 5-TotalIndexNow);
        TotalIndexNow += retrunlenth;
        // if (retrunlenth!=0) {
        //     //输出bufferforread
        //     for (int i = 0; i < TotalIndexNow; i++) {
        //         printf("%02x ", BufferForRead[i]);
        //     }
        //     printf("here3 %d\n",retrunlenth);
        //     fflush(stdout);
        // }
        TotalIndexNow=IRIS_Cut_Befor_Header(BufferForRead,TotalIndexNow);

        // if (retrunlenth!=10) {
        //     delay(1000);
        // }
    }
    int lenth = BufferForRead[3] * 256 + BufferForRead[4];
    while (TotalIndexNow < lenth + 7)
    {
        int sizeread=SerailRead(BufferForRead + TotalIndexNow, 10);
        //printf("Data from sensor: %d\n",sizeread);
        TotalIndexNow +=sizeread;
        if (sizeread!=10) {
            //delay(10);
        }
        //delay(10);
        //delay(10);
    }
    // 输出bufferforread
    printf("Data from sensor: ");
    int maxoutput= TotalIndexNow>50 ?  50 : TotalIndexNow;
    for (int i = 0; i <maxoutput; i++) {
        printf("%02x ", BufferForRead[i]);
    }
    printf("\n");

   TotalIndexNow= IRIS_Protocol_Unpack(BufferForRead,TotalIndexNow,Command,BufferForRead);
    return TotalIndexNow;

}

#include <chrono>

size_t GetInfoBackFromSensorinTime(uint8_t Command,uint32_t waittime) // big 是指用几个字节表示数据长度 暂时只发现采集数据时用两个字节
{
   delay(1);
    //获取当前电脑时间
    size_t begintime= get_system_uptime_ms();

    TotalIndexNow = 0;
    BufferForRead[0] = 0x00;
    memset(BufferForRead, 0, 200);


    size_t retrunlenth=SerailRead(BufferForRead + TotalIndexNow, 2);


    TotalIndexNow += retrunlenth;
    // TotalIndexNow=IRIS_Cut_Befor_Header(BufferForRead,TotalIndexNow);
    while (TotalIndexNow<5) {
        size_t retrunlenth=SerailRead(BufferForRead + TotalIndexNow, 5-TotalIndexNow);
        TotalIndexNow += retrunlenth;
        // if (retrunlenth!=0) {
        //     //输出bufferforread
        //     for (int i = 0; i < TotalIndexNow; i++) {
        //         printf("%02x ", BufferForRead[i]);
        //     }
        //     printf("here3 %d\n",retrunlenth);
        //     fflush(stdout);
        // }
        TotalIndexNow=IRIS_Cut_Befor_Header(BufferForRead,TotalIndexNow);
        if (get_system_uptime_ms()-begintime>waittime) return OVERTIME;
        // if (retrunlenth!=10) {
        //     delay(1000);
        // }
    }
    if (BufferForRead[2]==0x40) {
        printf("warning: sensor busy\n");
        return SENSORBUSY;

    }
    int lenth = BufferForRead[3] * 256 + BufferForRead[4];
    while (TotalIndexNow < lenth + 7)
    {
        int sizeread=SerailRead(BufferForRead + TotalIndexNow, 10);
        //printf("Data from sensor: %d\n",sizeread);
        TotalIndexNow +=sizeread;
        if (sizeread!=10) {
            //delay(10);
        }
        if (get_system_uptime_ms()-begintime>waittime) return OVERTIME;
        //delay(10);
        //delay(10);
    }
    // 输出bufferforread
    printf("Data from sensor: ");
    int maxoutput= TotalIndexNow>50 ?  50 : TotalIndexNow;
    for (int i = 0; i <maxoutput; i++) {
        printf("%02x ", BufferForRead[i]);
    }
    printf("\n");

    TotalIndexNow= IRIS_Protocol_Unpack(BufferForRead,TotalIndexNow,Command,BufferForRead);
    return TotalIndexNow;

}




size_t SerialWrite(u_char *data, size_t lenth)
{
    if (MySerialWrite != nullptr)
    {
        // Serial.println("init ok");
        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, SERIALWRITE readfunc)
{
    MySerialWrite = a;
    MySerialRead = readfunc;
    //std::string temp = "01";
    //a((u_char *)temp.c_str(), 2);
    isIS3Init = true;
}

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

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;
}

void CoverLittleandBigbyNumber(void *data, int lenth)
{
    char *tempdata = new char[lenth];
    memcpy(tempdata, data, lenth);
    for (size_t i = 0; i < lenth ; i++) {
        ((char *)data)[i] = tempdata[lenth-1-i];
    }
    delete[] tempdata;

}