#include "board.h"
#include "tdc_tof.h"

#define DFLT_WREG0   ((24u<<25) | \
                      ( 3u<<19) | \
                      ( 0u<<17) | \
                      ( 2u<<14) | \
                      ( 0u<<13) | \
                      ( 0u<<12) | \
                      ( 9u<< 8) | \
                      ( 0u<< 2) | \
                      ( 3u<< 0)) 

#define DFLT_WREG1   (( 1u<<31) | \
                      ( 0u<<30) | \
                      ( 1u<<29) | \
                      ( 0u<<28) | \
                      ( 0u<< 0))

#define DFLT_WREG2   (( 0u<<30) | \
                      ( 6u<<24) | \
                      ( 8u<<18) | \
                      (10u<<12) | \
                      (12u<< 6) | \
                      (14u<< 0))
                      
#define DFLT_WREG3   ((16u<<26) | \
                      (18u<<20) | \
                      (20u<<14))
                      
#define DFLT_WREG4   (( 0u<<28) | \
                      ( 0u<<27) | \
                      ( 1u<<26) | \
                      ( 1u<<25) | \
                      ( 0u<<24) | \
                      ( 1u<<23) | \
                      ( 0u<<22) | \
                      ( 1u<<21) | \
                      ( 0u<<20) | \
                      /*( 0u<<18) |*/ \
                      ( 3u<<18) | \
                      ( 0u<<17) | \
                      ( 0u<<14) | \
                      ( 0u<<11) | \
                      ( 1u<<10) | \
                      ( 1u<< 8))

uint16_t tof_state = TOF_ST_INIT;
uint32_t tof_flag;

//uint16_t CntSync = 0;

void tdc_tof_init(void)
{
    //MUX ADDA : PA08; ADDB : PC09
    mux_init();

    // MS1030 RSTN : PD02
    GpioInit(PD02_AF0,   GPIO_DIG_OUT_WEAK_PUSH_NONEUP_NONEDOWN);
    GpioClrPins(GpioPortD, GpioBitMsk2);   // reset active low
    us_delay(10);       // 10us > tPH(30ns)
    GpioSetPins(GpioPortD, GpioBitMsk2);   // reset release
    us_delay(10);       // 10us > tRFS(30ns)

    tdc_spi_init();     // peripheral init

    PeriClk_MutEnable(PeriClk_Spi1);
    DMA_ReqClkOn();

    // TDC POR
    tdc_spi_wr_code(0x50);
    us_delay(1000);     // 1000us > specification(500us)

    tdc_spi_wr_d32(0x80, DFLT_WREG0);
    
    tof_flag = TOF_FLAG_UNALGND_CHOP;
    if (tdc_spi_rd_d8(0xd3)!=(DFLT_WREG0&0xFFu)){
        tof_flag |= TOF_FLAG_TDC_FAULT;
    } else {
        // write_reg0
        tdc_spi_wr_d32(0x80, DFLT_WREG0 |
                              ( 2u<< 4)     // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us;
                      );

        // write_reg1
        tdc_spi_wr_d32(0x81, DFLT_WREG1 |
                            (5760u<< 7)     // DELVAL1=45us, format:16.5, time unit:Tref;
                      );

        // write_reg2
        tdc_spi_wr_d32(0x82, DFLT_WREG2 |
                              ( 0u<<31)     // EN_FIRST_WAVE, 0:disable; 1:enable;
                      );

        // write_reg3
        tdc_spi_wr_d32(0x83, DFLT_WREG3 |
                              ( 1u<<13)     // measure FW pulse width, 0:on; 1:off
                      );

        // write_reg4
        tdc_spi_wr_d32(0x84, DFLT_WREG4);

        tdc_spi_wr_code(0x70);
        tdc_spi_wr_code(0x06);

        // not 'INTN' but 'us_delay' to avoid dead loop when INTN fault
        us_delay(2000); // > 480 + 600 + 16 + 70
        tdc_read_cal(0x0u);
//            tof_flag ^= TOF_FLAG_UNALGND_CHOP;
//            // start a single ToF to align MS1030 internal calibration order(1 CAL every 24 ToF)
//            // must be sure that CAL not in the middle of UP Tof and DOWN Tof
//            tdc_start_tof(TDC_CFG_DO_WREG | TDC_CFG_FIRE_UP);
//
//            // not 'INTN' but 'us_delay' to avoid dead loop when INTN fault
//            us_delay(2000); // > 480 + 600 + 16 + 70
//
//            tdc_read_tof(TDC_CFG_FIRE_UP);  // read data and clear interrupt
    }

    DMA_ReqClkOff();
    PeriClk_MutDisable(PeriClk_Spi1);

    // MS1030 INTN : PD03 falling edge trigger
    GpioInit(PD03_AF0, GPIO_DIG_IN_PULLUP_NONEDOWN);
    GpioEventInit(PD03_AF0, GPIO_EVENT_FALL);

    // enable interrupt at NVIC side
    IRQMutEnable(PORTD_F_IRQn, NVIC_PRIO_1);
}

void tdc_start_tof(uint32_t cfg)
{
	//rt_enter_critical();
    uint32_t u32tmp;

    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    if (cfg & TDC_CFG_DO_WREG){
        // write_reg0
        tdc_spi_wr_d32(0x80, DFLT_WREG0 |
                              ( 1u<< 4)     // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us;
                      );

        // write_reg1
        // DN15: L/C > (42+21+10*2)*1000/1555=53us
        tdc_spi_wr_d32(0x81, DFLT_WREG1 |
                            (5760u<< 7)     // DELVAL1=45us, format:16.5, time unit:Tref;
//                            (6400u<< 7)   // DELVAL1=50us, format:16.5, time unit:Tref;
//                            (7040u<< 7)   // DELVAL1=55us, format:16.5, time unit:Tref;
//                            (7680u<< 7)   // DELVAL1=60us, format:16.5, time unit:Tref;
//                            (8230u<< 7)   // DELVAL1=65us, format:16.5, time unit:Tref;
//                            (8960u<< 7)   // DELVAL1=70us, format:16.5, time unit:Tref;
                      );

        // write_reg2
        tdc_spi_wr_d32(0x82, DFLT_WREG2 |
                              ( 1u<<31)     // EN_FIRST_WAVE, 0:disable; 1:enable;
                      );
    
        // write_reg3
        u32tmp = DFLT_WREG3 |
                  ( 0u<<13) |       // measure FW pulse width, 0:on; 1:off
                  (TdcFwOfst<<6);   // FW wave_offs, 0:0mv; 1~63:2~126mV; 64~127:-128~-2mV;
        if (tof_state&TOF_ST_VOID){
            u32tmp |= (1u<<13);     // disable FW pulse width to save power
        }
        tdc_spi_wr_d32(0x83, u32tmp);
    }

    // write_reg4
    u32tmp = DFLT_WREG4;
//    if (cfg & TDC_CFG_FIRE_UP){
//        u32tmp |= (1u<<30);     // FIRE_UP,   0:off; 1:on;
//    } else {
//        u32tmp |= (1u<<29);     // FIRE_DOWN, 0:off; 1:on;
//    }
		
		u32tmp |= (1u<<30);     // FIRE_UP,   0:off; 1:on;
		
    tdc_spi_wr_d32(0x84, u32tmp);

    // Initial TDC status regs
    tdc_spi_wr_code(0x70);

    // start single-direction measure
    tdc_spi_wr_code(0x03);

    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
//rt_exit_critical();
}

uint16_t TdcStatus;

uint16_t TdcUPwFw;
uint16_t TdcUPwHw;
uint32_t TdcUToF[9];

uint16_t TdcDPwFw;
uint16_t TdcDPwHw;
uint32_t TdcDToF[9];

uint32_t TdcCal;
//float TdcScale = 250.0f/65536;
float TdcScale = 0.00381147861f;

uint32_t TdcPT[4];
float TdcResist = 5000;     // OhmX10

float WaterPrmt[2] = {25.0f, 1496.6f};

void tdc_read_tof(uint32_t cfg)
{
		uint32_t ch = get_mux_ch();
//    uint16_t i;
    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    TdcStatus = tdc_spi_rd_d16(0xd2);   // status
    if ((TdcStatus&0x0600)==0x0000u){
        tof_flag ^= TOF_FLAG_UNALGND_CHOP;
//        if (CntSync>=240){
//            CntSync = 10;
//        } else {
//            CntSync += 10;
//        }
    }

    if ((TdcStatus&0x06F0)!=0x0090u){
        tof_flag |= TOF_FLAG_BLANK_PIPE;
        tdc_spi_wr_code(0x70);
    } 
		else 
		{
			TdcUPwFw = tdc_spi_rd_d16(0xd0);    // First wave pulse width
			TdcUPwHw = tdc_spi_rd_d16(0xd1);    // 1st Hit wave pulse width
			TdcUToF[ch] = tdc_spi_rd_d32(0xb8)*(1.0f/8);  // average of all
			TdcDToF[ch] = tdc_spi_rd_d32(0xC1)*(1.0f/8);  // average of all
			#if 0
        if (cfg & TDC_CFG_FIRE_UP){
            TdcUPwFw = tdc_spi_rd_d16(0xd0);    // First wave pulse width
            TdcUPwHw = tdc_spi_rd_d16(0xd1);    // 1st Hit wave pulse width
            tdc_spi_wr_code(0x70);

            // TOF_UP_HITs
            TdcUToF[0] = tdc_spi_rd_d32(0xb8)*(1.0f/8);  // average of all
//            for(i=0; i<8; i++){
//                TdcUToF[i+1] = tdc_spi_rd_d32(0xb0 + i);
//            };
        } else if (cfg & TDC_CFG_FIRE_DOWN){
            TdcDPwFw = tdc_spi_rd_d16(0xd0);    // First wave pulse width
            TdcDPwHw = tdc_spi_rd_d16(0xd1);    // 1st Hit wave pulse width
            tdc_spi_wr_code(0x70);

            // TOF_DOWN_HITs
            TdcDToF[0] = tdc_spi_rd_d32(0xb8)*(1.0f/8);  // average of all
//            for(i=0; i<8; i++){
//                TdcDToF[i+1] = tdc_spi_rd_d32(0xb0 + i);
//            };
        }
				#endif
    }

    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
}

void tdc_start_cal(uint32_t cfg)
{
//rt_enter_critical();
    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    // 8*T_32k = 244.140625us
    if (cfg & TDC_CFG_DO_WREG){
        // write_reg0
        tdc_spi_wr_d32(0x80, DFLT_WREG0 |
                              ( 2u<< 4)     // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us;
                      );

        // write_reg3
        tdc_spi_wr_d32(0x83, DFLT_WREG3 |
                              ( 1u<<13)         // measure FW pulse width, 0:on; 1:off
                      );
    }

    tdc_spi_wr_code(0x70);
    tdc_spi_wr_code(0x06);

    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
//rt_exit_critical();
}

void tdc_read_cal(uint32_t cfg)
{
	//rt_enter_critical();
    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    TdcStatus = tdc_spi_rd_d16(0xd2);   // status
    if ((TdcStatus&0x0600)==0x0000u){
        tof_flag ^= TOF_FLAG_UNALGND_CHOP;
//        if (CntSync>=240){
//            CntSync = 10;
//        } else {
//            CntSync += 10;
//        }
        tdc_spi_wr_code(0x70);
        TdcCal = tdc_spi_rd_d32(0xd4);

//        TdcScale = 244140.625f/TdcCal;
        TdcScale += (244140.625f/TdcCal - TdcScale)*(1.0f/2);
				//LOG("----------TdcScale=%.6f\n",TdcScale);
    } else {
        tdc_spi_wr_code(0x70);
    }

    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
//rt_exit_critical();
}

void tdc_start_temp(uint32_t cfg)
{
//rt_enter_critical();
    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    if (cfg & TDC_CFG_DO_WREG){
        // write_reg0
        tdc_spi_wr_d32(0x80, DFLT_WREG0 |
                              ( 3u<< 4)     // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; 3:512us; ~ 6:4096us;
                      );

        // write_reg1
        tdc_spi_wr_d32(0x81, DFLT_WREG1 |
                              ( 0u<< 7)   // DELVAL1=0us, format:16.5, time unit:Tref;
                      );

        // write_reg3
        tdc_spi_wr_d32(0x83, DFLT_WREG3 |
                              ( 1u<<13)         // measure FW pulse width, 0:on; 1:off
                      );
    }

    tdc_spi_wr_code(0x70);
    tdc_spi_wr_code(0x04);

    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
//rt_exit_critical();
}

void tdc_read_temp(uint32_t cfg)
{
//rt_enter_critical();
    if (cfg & TDC_CFG_DO_CKENA){
        PeriClk_MutEnable(PeriClk_Spi1);
        DMA_ReqClkOn();
    }

    TdcStatus = tdc_spi_rd_d16(0xd2);   // status
    tdc_spi_wr_code(0x70);
    TdcPT[0] = tdc_spi_rd_d32(0xc2);  // REF 5K +/- 5
    TdcPT[1] = tdc_spi_rd_d32(0xc3);  // U7
    //TdcPT[2] = tdc_spi_rd_d32(0xc4);  // U8
    //TdcPT[3] = tdc_spi_rd_d32(0xc5);  // REF 1.5K

//    if (TdcPT[0]>0x1000000u && TdcPT[0]<0x2000000u){
//        TdcResist = 5000.0f*TdcPT[1]/TdcPT[0];
//        Ohm2Dgr2USSpd(TdcResist, WaterPrmt);        // [0]:Temperature; [1]:ultrasonic speed @Temp
//    }
    if (TdcPT[0]>0xB00000u && TdcPT[0]<0xE00000u){
        TdcResist = 2500.0f*TdcPT[1]/TdcPT[0];  // ohmX10
        Ohm2Dgr2USSpd(TdcResist, WaterPrmt);        // [0]:Temperature; [1]:ultrasonic speed @Temp
    }
		LOG("----------T=%.6f,C=%.6f\n",WaterPrmt[0],WaterPrmt[1]);
    if (cfg & TDC_CFG_DO_CKDIS){
        DMA_ReqClkOff();
        PeriClk_MutDisable(PeriClk_Spi1);
    }
//rt_exit_critical();
}