//
// Created by Nick on 2019/4/7.
//
#include "FpgaComm.h"

#include <thread>
#include "../uart/uartregsaccess.h"
#include <iostream>
#include <uart/Gpio.h>
#include <base/utils.h>

#define Reg(x) (fpgaParams.x)
#define RegIndex(x)  (std::distance((unsigned int*)&fpgaParams, (unsigned int*)&fpgaParams.x))
#define ReadReg(x) read(RegIndex(x), *((unsigned int*)&fpgaParams.x))
#define WriteReg(x) write(RegIndex(x), *((unsigned int*)&fpgaParams.x))
#define WR_Reg(x) (WriteReg(x),ReadReg(x))

#define CO(x) std::cout << #x << " : "<< RegIndex(x) << std::endl

FpgaComm::controller::controller()
{
    status_.reset(new Gpio(PORT_STATUS));
    cfg_.reset(new Gpio(PORT_CONFIG));
    reload_.reset(new Gpio(PORT_RELOAD));
    reset_.reset(new GpioOut(PORT_RESET));

    vdd_3voff_.reset(new GpioOut(PORT_VDD_3VOFF));
    vdd_5ven_.reset(new GpioOut(PORT_VDD_5VEN));
    init_done_.reset(new Gpio(PORT_INIT_DONE));
    img_tx_.reset(new Gpio(PORT_IMAGE_TX));
}
FpgaComm::controller::~controller()
{}

void FpgaComm::controller::reset(void)
{
    reset_->setValue(Gpio::Low);
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    utils::to_log(LOG_LEVEL_DEBUG, "FPGA-reset set Low: %d\n", status_->getValue());

    reset_->setValue(Gpio::High);
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    utils::to_log(LOG_LEVEL_DEBUG, "FPGA-reset set High: %d\n", status_->getValue());
}
void FpgaComm::controller::reload(void)
{
    // fpga 代码重载
    reload_->setValue(Gpio::Low);
    std::this_thread::sleep_for(std::chrono::milliseconds(15));

    cfg_->setValue(Gpio::Low);
    std::this_thread::sleep_for(std::chrono::milliseconds(15));
    utils::to_log(LOG_LEVEL_DEBUG, "FPGA-reload set Low: %d\n", status_->getValue());
    
    reload_->setValue(Gpio::High);
    std::this_thread::sleep_for(std::chrono::milliseconds(15));
    cfg_->setValue(Gpio::High);
    utils::to_log(LOG_LEVEL_DEBUG, "FPGA-reload set High: %d\n", status_->getValue());
    
    std::this_thread::sleep_for(std::chrono::seconds(3));
    utils::to_log(LOG_LEVEL_DEBUG, "FPGA-reload set High after 3 seconds: %d\n", status_->getValue());
}

FpgaComm::FpgaComm(int bauds, bool query) : bauds_(bauds)
{
    controller_.reset(new controller());
    m_regsAccess.reset(new UartRegsAccess(FPGA_UART, bauds_, 0x03, 0x83));
    if(query)
        return;

    ok_ = update();
    if(ok_)
    {
        Reg(AledR).sample = 256;
        WR_Reg(AledR);

        controller_->reset();
        resetADC();
    }
}

bool FpgaComm::read(unsigned int addr, unsigned int& val) {
    return m_regsAccess->read(addr, val);
}

bool FpgaComm::write(unsigned int addr, unsigned int val) {
    return m_regsAccess->write(addr, val);
}

void FpgaComm::regsAccess_reset(bool enable){
    if(enable)
    {   
        if(!m_regsAccess.get())
            m_regsAccess.reset(new UartRegsAccess(FPGA_UART, bauds_, 0x03, 0x83));
        update();
        return;
    }
    if(m_regsAccess.get())
        m_regsAccess.reset();
} 

void FpgaComm::setFrameHeight(int height){
	Reg(frame).height = height;
    WR_Reg(frame);
}

int FpgaComm::getFrameHeight()
{
   	return Reg(frame).height; 
}
int FpgaComm::get_real_height(void)
{
    unsigned int val;
    unsigned int reg8 = 0;

    this->read(8, reg8);

    this->read(14, val);
    int regv = val;
    val &= 0x0000ffff;

    this->write(8, reg8 & 0xfffffff7);

    std::this_thread::sleep_for(std::chrono::milliseconds(5));

    this->read(14, val);
    regv = val;
    val &= 0x0000ffff;
    this->read(8, reg8);

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    this->write(8, reg8 | 0x8);

    return val;
}

void FpgaComm::setFrameNum(int num){
	Reg(frame).num = num;
	WR_Reg(frame);
}

void FpgaComm::enableLed(bool bEnable) {
    Reg(AledR).ledEnable = bEnable;
    WR_Reg(AledR);
#ifdef HAS_UV
	Reg(BledR).ledEnable = bEnable;
	WR_Reg(BledR);
#else
	Reg(BledR).ledEnable = bEnable;
	WR_Reg(BledR);
#endif
}

void FpgaComm::enableUV(bool enable){
#ifdef HAS_UV
	isUVEnable = enable;
	Reg(BledR).ledEnable = isUVEnable;
    WR_Reg(BledR);
#endif
}

void FpgaComm::capture() {
    Reg(cmd).cmd = 0;
    WriteReg(cmd);
    Reg(cmd).cmd = 1;
    WriteReg(cmd);
}

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

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

void FpgaComm::setAGain(int indexGain, int value) {
    int index = indexGain;
    int val;// = value
    value>=511?val=511:val=value;
    index++;
    fpgaParams.Aad.ad0_addr = index*2;
    fpgaParams.Aad.ad1_addr = fpgaParams.Aad.ad0_addr  + 1;
    fpgaParams.Aad.ad0_value = val;
    fpgaParams.Aad.ad1_value = val>255?1:0;
    fpgaParams.Aad.ad0_rw = 0;
    fpgaParams.Aad.ad1_rw = 0;
    write(0x04,*(int*)&fpgaParams.Aad);
    Reg(mode).adcA = 1;
    WriteReg(mode);
    Reg(mode).adcA = 0;
    WriteReg(mode);
}

void FpgaComm::setBGain(int indexGain, int value) {
    int index = indexGain;
    int val;// = value
    value>=511?val=511:val=value;
    index++;
    fpgaParams.Bad.ad0_addr = index*2;
    fpgaParams.Bad.ad1_addr = index*2 + 1;
    fpgaParams.Bad.ad0_value = val;
    fpgaParams.Bad.ad1_value = val>255?1:0;
    fpgaParams.Bad.ad0_rw = 0;
    fpgaParams.Bad.ad1_rw = 0;
    write(0x07,*(int*)&fpgaParams.Bad);
    Reg(mode).adcB = 1;
    WriteReg(mode);
    Reg(mode).adcB = 0;
    WriteReg(mode);
}

void FpgaComm::setAOffset(int indexOffset, int value) {
    Reg(Aad).ad0_rw = 0;
    Reg(Aad).ad1_rw = 0;
    Reg(Aad).ad0_addr = indexOffset + 0x0e;
    Reg(Aad).ad1_addr = 0x14;
    Reg(Aad).ad0_value = value;
    WriteReg(Aad);
    Reg(mode).adcA = 1;
    WriteReg(mode);
    Reg(mode).adcA = 0;
    WriteReg(mode);
}

void FpgaComm::setBOffset(int indexOffset, int value) {
    Reg(Bad).ad0_rw = 0;
    Reg(Bad).ad1_rw = 0;
    Reg(Bad).ad0_addr = indexOffset + 0x0e;
    Reg(Bad).ad1_addr = 0x14;
    Reg(Bad).ad0_value = value;
    WriteReg(Bad);
    Reg(mode).adcB = 1;
    WriteReg(mode);
    Reg(mode).adcB = 0;
    WriteReg(mode);
}

void FpgaComm::setAExposureR(int value) {
    Reg(AledR).ledR = value;
    WR_Reg(AledR);
}

void FpgaComm::setAExposureG(int value) {
    Reg(AledGB).ledG = value;
    WR_Reg(AledGB);
}

void FpgaComm::setAExposureB(int value) {
    Reg(AledGB).ledB = value;
    WR_Reg(AledGB);
}

void FpgaComm::setAExposureUV(int value){
#ifdef HAS_UV	
	Reg(UVLed).ledASide = value;
    WR_Reg(UVLed);
#endif
}

void FpgaComm::setBExposureR(int value) {
    Reg(BledR).ledR = value;
    WR_Reg(BledR);
}

void FpgaComm::setBExposureG(int value) {
    Reg(BledGB).ledG = value;
    WR_Reg(BledGB);
}

void FpgaComm::setBExposureB(int value) {
    Reg(BledGB).ledB = value;
    WR_Reg(BledGB);
}

void FpgaComm::setBExpousreUV(int value){
#ifdef HAS_UV
	Reg(UVLed).ledBSide = value;
	WR_Reg(UVLed);
#endif
}

void FpgaComm::setSp(int value)
{
    Reg(mode).sp = value;
	WR_Reg(mode);
}

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

void FpgaComm::setColorMode(int  mode)
{
    Reg(mode).colorMode = mode;  
    WR_Reg(mode);
}

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

void FpgaComm::setSample(int sample)
{
    Reg(mode).sample = sample;
    WR_Reg(mode);
    Reg(AledR).sample  = sample;  
    WR_Reg(AledR);
}

 void FpgaComm::EnableTest(bool bTest)
 {
    Reg(mode).selftest = bTest; 
    WR_Reg(mode);
 }

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

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

void FpgaComm::setDpi(int dpi)
{
    Reg(mode).dpi = dpi;
    WR_Reg(mode);
}

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

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

bool FpgaComm::update(std::vector<int>* data)
{
    bool        good = true;
    std::vector<int>    d;
    chronograph watch;

    if(!data)
        data = &d;

    utils::to_log(LOG_LEVEL_DEBUG, "Read %u FpgaComm registers ...\n", MAX_REGS);
    for(int i = 0; i < MAX_REGS; i++)
    {
        good = read(i, fpgaParams.regs[i]);
        if(!good)
        {
            utils::to_log(LOG_LEVEL_FATAL, " FATAL: Register %d(0x%08x) timeout.\n", i, fpgaParams.regs[i]);
            break;
        }
        data->push_back(fpgaParams.regs[i]);
        utils::to_log(LOG_LEVEL_DEBUG, " reg(%02d) = 0x%08x\n", i, fpgaParams.regs[i]);
    }
    utils::to_log(LOG_LEVEL_DEBUG, "Read FpgaComm registers over.\n");

    return good;
}

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

void FpgaComm::resetADC() {

    fpgaParams.Aad.ad0_rw = 0;
    fpgaParams.Aad.ad0_addr = 0;
    fpgaParams.Aad.ad0_value = 0;
    fpgaParams.Aad.ad1_rw = 0;
    fpgaParams.Aad.ad1_addr = 0;
    fpgaParams.Aad.ad1_value = 0;
    WR_Reg(Aad);
    fpgaParams.mode.adcA = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcA = 0;
    WR_Reg(mode);
    fpgaParams.Bad.ad0_rw = 0;
    fpgaParams.Bad.ad0_addr = 0;
    fpgaParams.Bad.ad0_value = 0;
    fpgaParams.Bad.ad1_rw = 0;
    fpgaParams.Bad.ad1_addr = 0;
    fpgaParams.Bad.ad1_value = 0;
    WR_Reg(Bad);
    fpgaParams.mode.adcB = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcB = 0;
    WR_Reg(mode);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    fpgaParams.Aad.ad0_rw = 0;
    fpgaParams.Aad.ad0_addr = 0;
    fpgaParams.Aad.ad0_value = 7;
    fpgaParams.Aad.ad1_rw = 0;
    fpgaParams.Aad.ad1_addr = 0;
    fpgaParams.Aad.ad1_value = 7;
    WR_Reg(Aad);
    fpgaParams.mode.adcA = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcA = 0;
    WR_Reg(mode);
    fpgaParams.Bad.ad0_rw = 0;
    fpgaParams.Bad.ad0_addr = 0;
    fpgaParams.Bad.ad0_value = 7;
    fpgaParams.Bad.ad1_rw = 0;
    fpgaParams.Bad.ad1_addr = 0;
    fpgaParams.Bad.ad1_value = 7;
    WR_Reg(Bad);
    fpgaParams.mode.adcB = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcB = 0;
    WR_Reg(mode);
    fpgaParams.Aad.ad0_rw = 0;
    fpgaParams.Aad.ad0_addr = 1;
    fpgaParams.Aad.ad0_value = 0x50;
    fpgaParams.Aad.ad1_rw = 0;
    fpgaParams.Aad.ad1_addr = 1;
    fpgaParams.Aad.ad1_value = 0x50;
    WR_Reg(Aad);
    fpgaParams.mode.adcA = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcA = 0;
    WR_Reg(mode);
    fpgaParams.Bad.ad0_rw = 0;
    fpgaParams.Bad.ad0_addr = 1;
    fpgaParams.Bad.ad0_value = 0x50;
    fpgaParams.Bad.ad1_rw = 0;
    fpgaParams.Bad.ad1_addr = 1;
    fpgaParams.Bad.ad1_value = 0x50;
    WR_Reg(Bad);
    fpgaParams.mode.adcB = 1;
    WR_Reg(mode);
    fpgaParams.mode.adcB = 0;
    WR_Reg(mode);
}

bool FpgaComm::is_ok(void)
{
    return ok_;
}

void FpgaComm::setVsp(unsigned int Aside,unsigned int BSide)
{
    CISVSP vsp;
    vsp.bits.ASide_VSP = Aside;
    vsp.bits.BSide_VSP = BSide;
    vsp.bits.reserved=0;

    this->write(13, vsp.value);
}