//
// Created by Nick on 2019/4/7.
//

#include <iostream>
#include "FpgaComm.h"
#include <thread>

#define LOG_TAG "FpgaComm"
#define LOGD(...) ((void)printf(__VA_ARGS__))

int FpgaComm::read(int addr)
{
    unsigned char *pdata = bufRecv;
    pdata[0] = 0x03;
    pdata[1] = (unsigned char)addr;
    m_serial.Write(bufRecv, 2);
    if (m_serial.Read(bufRecv, 5, 300))
    {
        int ret = 0;
        unsigned char *pdata = (unsigned char *)(&ret);
        for (int i = 0; i < 4; i++)
        {
            pdata[i] = bufRecv[4 - i];
        }
        return ret;
    }

    return -1;
}

void FpgaComm::write(int addr, int data)
{
    unsigned char *pdata = bufSend;
    pdata[0] = 0x83;
    pdata[1] = (unsigned char)addr;
    unsigned char *idata = (unsigned char *)&data;
    for (int i = 0; i < 4; i++)
    {
        pdata[6 - i] = idata[i];
    }
    m_serial.Write(bufSend, sizeof(bufSend));
}

void FpgaComm::updateRegs(int addr)
{
    write(addr, fpgaParams.regs[addr]);
}

void FpgaComm::setFrameHeight(int height)
{
    fpgaParams.params.frame.height = height;
    write(0x00, fpgaParams.regs[0x00]);
}

int FpgaComm::getFrameHeight()
{
    // fpgaParams.params.frame.height = height;
    int reg0v = read(0x00);
    FrameFpga *frameinfo = (FrameFpga *)&reg0v;
    return frameinfo->height;
}

int FpgaComm::getAutoFrameHeight()
{
    unsigned int val;
    unsigned int reg8 = 0;
    reg8 = read(0x08);
    val = read(14);
    int regv = val;
    val &= 0x0000ffff;
    write(0x8, reg8 & 0xfffffff7);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    // val = read(14);
    // regv = val;
    // val &= 0x0000ffff;
    // reg8 = read(0x8);
    // LOG("TWO height = %d reg[14] = %d \n", val, regv);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    write(0x8, reg8 | 0x8);
    return val;
}

void FpgaComm::setFrameNum(int num)
{
    fpgaParams.params.frame.num = num;
    write(0x00, fpgaParams.regs[0x00]);
}

void FpgaComm::enableLed(bool bEnable)
{
    fpgaParams.params.AledR.user_define.led_sample.ledEnable = bEnable;
    write(0x05, fpgaParams.regs[0x05]);
#ifdef HAS_UV
    fpgaParams.params.BledR.user_define.led_sample.ledEnable = bEnable;
    write(0x08, fpgaParams.regs[0x08]);
#else
    fpgaParams.params.BledR.user_define.led_sample.ledEnable = 0;
    write(0x08, fpgaParams.regs[0x08]);
#endif
}

void FpgaComm::enableUV(bool enable)
{
#ifdef HAS_UV
    isUVEnable = enable;
    fpgaParams.params.BledR.user_define.led_sample.ledEnable = isUVEnable;
    write(0x08, fpgaParams.regs[0x08]);
#endif
}

void FpgaComm::capture()
{ // ������ʲô�أ�
    fpgaParams.params.cmd.cmd = 0;
    write(0x02, fpgaParams.regs[0x02]);
    fpgaParams.params.cmd.cmd = 1;
    write(0x02, fpgaParams.regs[0x02]);
}

int FpgaComm::getRegs(int addr)
{
    return fpgaParams.regs[addr];
}

void FpgaComm::setRegs(int addr, int value)
{
    fpgaParams.regs[addr] = value;
    write(addr, value);
}

FpgaComm::FpgaComm() : fanGpio(FAN_PORT) // ��ʼ��ʱ����ȡ�Ĵ�������Ϣ
{
    m_serial.Open(com.c_str(), bauds);
    // for(int i = 0; i < MAX_REGS; i++){
    //     fpgaParams.regs[i] = read(i);
    //     LOG("reg[%d] = 0x%08x \n", i, fpgaParams.regs[i]);
    // }
    fpgaParams.params.AledR.user_define.led_sample.sample = 256;
    updateRegs(0x05);

    enableLed(true);
}

void FpgaComm::updateRegs()
{
    for (int i = 0x05; i < MAX_REGS; i++)
    {
        updateRegs(i);
    }
    fpgaParams.params.AledR.user_define.led_sample.sample = 256;
    updateRegs(0x05);
}

void FpgaComm::setAGain(int indexGain, int value)
{
    AdGain adGain;
    adGain.value = value;
    indexGain++;
    fpgaParams.params.Aad.bits.ad0_addr = indexGain * 2;
    fpgaParams.params.Aad.bits.ad1_addr = fpgaParams.params.Aad.bits.ad0_addr + 1;
    fpgaParams.params.Aad.bits.ad0_value = adGain.gain_value.gain_low8;
    fpgaParams.params.Aad.bits.ad1_value = adGain.gain_value.gain_hight;
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
}

void FpgaComm::getAGain(int indexGain, int &Avalue, int &Bvalue)
{
    AdGain adGain;
    CisAdGain adgain;
    // adGain.value = value;
    indexGain++;
    fpgaParams.params.Aad.bits.ad0_addr = indexGain * 2;
    fpgaParams.params.Aad.bits.ad1_addr = fpgaParams.params.Aad.bits.ad0_addr + 1;
    fpgaParams.params.Aad.bits.ad0_value = 0;
    fpgaParams.params.Aad.bits.ad1_value = 0;
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
    auto ret = read(0x03);
    adgain.value = ret;
    Avalue = adgain.bits.ad0_value;
    Bvalue = adgain.bits.ad1_value;
}

void FpgaComm::setBGain(int indexGain, int value)
{
    AdGain adGain;
    adGain.value = value;
    indexGain++;
    fpgaParams.params.Bad.bits.ad0_addr = indexGain * 2;
    fpgaParams.params.Bad.bits.ad1_addr = indexGain * 2 + 1;
    fpgaParams.params.Bad.bits.ad0_value = adGain.gain_value.gain_low8;
    fpgaParams.params.Bad.bits.ad1_value = adGain.gain_value.gain_hight;
    fpgaParams.params.Bad.bits.ad0_rw = 0;
    fpgaParams.params.Bad.bits.ad1_rw = 0;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
}

void FpgaComm::getBGain(int indexGain, int &Avalue, int &Bvalue)
{
    AdGain adGain;
    CisAdGain adgain;
    // adGain.value = value;
    indexGain++;
    fpgaParams.params.Bad.bits.ad0_addr = indexGain * 2;
    fpgaParams.params.Bad.bits.ad1_addr = indexGain * 2 + 1;
    fpgaParams.params.Bad.bits.ad0_value = adGain.gain_value.gain_low8;
    fpgaParams.params.Bad.bits.ad1_value = adGain.gain_value.gain_hight;
    fpgaParams.params.Bad.bits.ad0_rw = 1;
    fpgaParams.params.Bad.bits.ad1_rw = 1;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
    auto ret = read(0x03);
    adgain.value = ret;
    Avalue = adgain.bits.ad0_value;
    Bvalue = adgain.bits.ad1_value;
}

void FpgaComm::setAOffset(int indexOffset, int value)
{
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    fpgaParams.params.Aad.bits.ad0_addr = indexOffset + 0x0e;
    fpgaParams.params.Aad.bits.ad1_addr = 0x14;
    fpgaParams.params.Aad.bits.ad1_value = 0x50;
    fpgaParams.params.Aad.bits.ad0_value = value;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
}

void FpgaComm::getAOffset(int indexGain, int &Avalue, int &Bvalue)
{
    fpgaParams.params.Aad.bits.ad0_rw = 1;
    fpgaParams.params.Aad.bits.ad1_rw = 1;
    fpgaParams.params.Aad.bits.ad0_addr = indexGain + 0x0e;
    fpgaParams.params.Aad.bits.ad1_addr = 0x14;
    fpgaParams.params.Aad.bits.ad1_value = 0x50;
    fpgaParams.params.Aad.bits.ad0_value = 0;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
    CisAdGain adgain;
    auto ret = read(0x03);
    adgain.value = ret;
    Avalue = adgain.bits.ad0_value;
    Bvalue = adgain.bits.ad1_value;
}

void FpgaComm::setBOffset(int indexOffset, int value)
{
    fpgaParams.params.Bad.bits.ad0_rw = 0;
    fpgaParams.params.Bad.bits.ad1_rw = 0;
    fpgaParams.params.Bad.bits.ad0_addr = indexOffset + 0x0e;
    fpgaParams.params.Bad.bits.ad1_addr = 0x14;
    fpgaParams.params.Aad.bits.ad1_value = 0x50;
    fpgaParams.params.Bad.bits.ad0_value = value;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
}

void FpgaComm::getBOffset(int indexGain, int &Avalue, int &Bvalue)
{
    fpgaParams.params.Bad.bits.ad0_rw = 1;
    fpgaParams.params.Bad.bits.ad1_rw = 1;
    fpgaParams.params.Bad.bits.ad0_addr = indexGain + 0x0e;
    fpgaParams.params.Bad.bits.ad1_addr = 0x14;
    fpgaParams.params.Aad.bits.ad1_value = 0x50;
    fpgaParams.params.Bad.bits.ad0_value = 0;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
    CisAdGain adgain;
    auto ret = read(0x03);
    adgain.value = ret;
    Avalue = adgain.bits.ad0_value;
    Bvalue = adgain.bits.ad1_value;
}

void FpgaComm::setAExposureR(int value)
{
    fpgaParams.params.AledR.ledR = value;
    updateRegs(0x05);
    fpgaParams.regs[0x05] = read(0x05);
}

void FpgaComm::setAExposureG(int value)
{
    fpgaParams.params.AledGB.bits.ledG = value;
    updateRegs(0x06);
    fpgaParams.regs[0x06] = read(0x06);
}

void FpgaComm::setAExposureB(int value)
{
    fpgaParams.params.AledGB.bits.ledB = value;
    updateRegs(0x06);
    fpgaParams.regs[0x06] = read(0x06);
}

void FpgaComm::setAExposureUV(int value)
{
#ifdef HAS_UV
    fpgaParams.params.UVLed.bits.ledASide = value;
    updateRegs(0x0d);
    fpgaParams.regs[0x0d] = read(0x0d);
#endif
}

void FpgaComm::setBExposureR(int value)
{
    fpgaParams.params.BledR.ledR = value;
    updateRegs(0x08);
    fpgaParams.regs[0x08] = read(0x08);
}

void FpgaComm::setBExposureG(int value)
{
    fpgaParams.params.BledGB.bits.ledG = value;
    updateRegs(0x09);
    fpgaParams.regs[0x09] = read(0x09);
}

void FpgaComm::setBExposureB(int value)
{
    fpgaParams.params.BledGB.bits.ledB = value;
    updateRegs(0x09);
    fpgaParams.regs[0x09] = read(0x09);
}

void FpgaComm::setBExpousreUV(int value)
{
#ifdef HAS_UV
    fpgaParams.params.UVLed.bits.ledBSide = value;
    updateRegs(0x0d);
    fpgaParams.regs[0x0d] = read(0x0d);
#endif
}

void FpgaComm::setEnTestCol(bool en)
{
    fpgaParams.params.AledR.user_define.led_sample.en_test_color = en ? 1 : 0;
    updateRegs(0x05);
    fpgaParams.regs[0x05] = read(0x05);
}

void FpgaComm::setEnTestBit(bool en)
{
    fpgaParams.params.AledR.user_define.led_sample.en_test = en ? 1 : 0;
    updateRegs(0x05);
    fpgaParams.regs[0x05] = read(0x05);
}

void FpgaComm::setFanMode(int mode)
{
    fanGpio.setValue(mode == 3 ? Gpio::Low : Gpio::High);
}

void FpgaComm::setSp(int value)
{
    setRegs(0x01, value);
}

int FpgaComm::getSp()
{
    return fpgaParams.params.mode.sp;
}

void FpgaComm::setColorMode(int mode)
{
    fpgaParams.params.mode.colorMode = mode;
    updateRegs(0x01);
}

int FpgaComm::getColorMode()
{
    return fpgaParams.params.mode.colorMode;
}

void FpgaComm::setSample(int sample)
{
    fpgaParams.params.mode.sample = sample;
    updateRegs(0x01);
    fpgaParams.params.AledR.user_define.led_sample.sample = sample;
    updateRegs(0x05);
}

void FpgaComm::EnableTest(bool bTest)
{
    fpgaParams.params.mode.selftest = bTest;
    updateRegs(0x01);
}

int FpgaComm::IsTest()
{
    return fpgaParams.params.mode.selftest;
}

int FpgaComm::getSample()
{
    return fpgaParams.params.mode.sample;
}

void FpgaComm::setDpi(int dpi)
{
    fpgaParams.params.mode.dpi = dpi;
    updateRegs(0x01);
}

int FpgaComm::getDpi()
{
    return fpgaParams.params.mode.dpi;
}

void FpgaComm::setSample(int sampleFront, int sampleBack)
{
    fpgaParams.params.mode.sample = sampleFront;
    updateRegs(0x01);
    fpgaParams.params.AledR.user_define.led_sample.sample = sampleBack;
    updateRegs(0x05);
}

// 20190626 YHP autoTrig function
void FpgaComm::setTrigMode(bool isArmMode)
{
    int tmp = read(0x0b);
    if (!isArmMode)
    { // default value+ ARM MODE,bit27 =0;
        fpgaParams.params.TrigMode = tmp & 0XFBFFFFFF;
    }
    else
    {
        fpgaParams.params.TrigMode = tmp | (1 << 26);
    }
    updateRegs(0x0b);
}

// void FpgaComm::setDelayTime(int value) {
//     // fpgaParams.params.DelayTime = value;
//     // updateRegs(0x0C);
// }
void FpgaComm::setFrame_interval_min(int min)
{
    fpgaParams.params.FrameInterval.Frame_Interval_min = min;
    fpgaParams.params.FrameInterval.reversed = 0;
    updateRegs(0xc);
}

int FpgaComm::getFrame_interval_min()
{
    fpgaParams.params.FrameInterval.reversed = 0;
    auto value = read(0x0c);
    fpgaParams.params.FrameInterval = *(CisFrameInterval *)&value;
    return fpgaParams.params.FrameInterval.Frame_Interval_min;
}

void FpgaComm::setFrame_interval_max(int max)
{
    fpgaParams.params.FrameInterval.Frame_Interval_max = max;
    fpgaParams.params.FrameInterval.reversed = 0;
    updateRegs(0xc);
}

int FpgaComm::getFrame_interval_max()
{
    fpgaParams.params.FrameInterval.reversed = 0;
    auto value = read(0x0c);
    fpgaParams.params.FrameInterval = *(CisFrameInterval *)&value;
    return fpgaParams.params.FrameInterval.Frame_Interval_max;
}

// ���¼Ĵ�����Ϣ
void FpgaComm::update()
{
    for (int i = 0; i < MAX_REGS; i++)
    {
        fpgaParams.regs[i] = read(i);
        LOG("reg[%d] = 0x%08x \n", i, fpgaParams.regs[i]);
    }
}

void FpgaComm::enableJamCheck(bool b)
{
    // fpgaParams.params.BledR.user_define.led_sample.jamEnable = b;
    // updateRegs(0x08);
}

void FpgaComm::resetADC()
{
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad0_addr = 0;
    fpgaParams.params.Aad.bits.ad0_value = 0;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    fpgaParams.params.Aad.bits.ad1_addr = 0;
    fpgaParams.params.Aad.bits.ad1_value = 0;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
    fpgaParams.params.Bad.bits.ad0_rw = 0;
    fpgaParams.params.Bad.bits.ad0_addr = 0;
    fpgaParams.params.Bad.bits.ad0_value = 0;
    fpgaParams.params.Bad.bits.ad1_rw = 0;
    fpgaParams.params.Bad.bits.ad1_addr = 0;
    fpgaParams.params.Bad.bits.ad1_value = 0;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad0_addr = 0;
    fpgaParams.params.Aad.bits.ad0_value = 7;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    fpgaParams.params.Aad.bits.ad1_addr = 0;
    fpgaParams.params.Aad.bits.ad1_value = 7;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
    fpgaParams.params.Bad.bits.ad0_rw = 0;
    fpgaParams.params.Bad.bits.ad0_addr = 0;
    fpgaParams.params.Bad.bits.ad0_value = 7;
    fpgaParams.params.Bad.bits.ad1_rw = 0;
    fpgaParams.params.Bad.bits.ad1_addr = 0;
    fpgaParams.params.Bad.bits.ad1_value = 7;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
    fpgaParams.params.Aad.bits.ad0_rw = 0;
    fpgaParams.params.Aad.bits.ad0_addr = 1;
    fpgaParams.params.Aad.bits.ad0_value = 0x50;
    fpgaParams.params.Aad.bits.ad1_rw = 0;
    fpgaParams.params.Aad.bits.ad1_addr = 1;
    fpgaParams.params.Aad.bits.ad1_value = 0x50;
    updateRegs(0x04);
    fpgaParams.params.mode.adcA = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcA = 0;
    updateRegs(0x01);
    fpgaParams.params.Bad.bits.ad0_rw = 0;
    fpgaParams.params.Bad.bits.ad0_addr = 1;
    fpgaParams.params.Bad.bits.ad0_value = 0x50;
    fpgaParams.params.Bad.bits.ad1_rw = 0;
    fpgaParams.params.Bad.bits.ad1_addr = 1;
    fpgaParams.params.Bad.bits.ad1_value = 0x50;
    updateRegs(0x07);
    fpgaParams.params.mode.adcB = 1;
    updateRegs(0x01);
    fpgaParams.params.mode.adcB = 0;
    updateRegs(0x01);
}

void FpgaComm::setVsp(unsigned int Aside, unsigned int BSide)
{
    // auto ret = read(13);
    CISVSP vsp; // = *(CISVSP*)&ret;
    vsp.bits.ASide_VSP = Aside;
    vsp.bits.BSide_VSP = BSide;
    vsp.bits.reserved = 0;
    printf("setVsp A side =%d  B side=%d  vspint=%08x  \n", vsp.bits.ASide_VSP, vsp.bits.BSide_VSP, vsp.value);
    write(13, vsp.value);
}

unsigned int FpgaComm::getFrame_counter_val()
{    
    int crt_frame_count=0;
    unsigned int reg8 = 0;

    reg8 = read(8);
    //write(8,reg8 & 0x8);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    write(8, reg8 & 0xfffffff7);//off stop snap
    reg8 = read(8);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    crt_frame_count = read(0x10);

    if(crt_frame_count <= 0)//读取失败或帧数为0的情况下，多读几次
    {
        for(int i = 0;i<3;i++)
        {
            crt_frame_count = read(0x10);
            printf("try %d times read frame count,frame_count = %d \n",i,crt_frame_count);
            if(crt_frame_count > 0)
                break;
        }
    }

    write(8, reg8 | 0x8);// on  reset counter
    std::this_thread::sleep_for(std::chrono::milliseconds(2));
    //printf("TWO height reg[14] = %d  reg[16] = %d  \n", read(14) & 0xffff,read(16));
    return crt_frame_count;
}

int FpgaComm::getFrameNum(){
    return fpgaParams.params.frame.num;
}

void FpgaComm::set_cis_type(bool isA3_CIS)
{
    fpgaParams.params.AledR.user_define.led_sample.cis_type = isA3_CIS?1:0;
}