#include "zf_common_headfile.h"
#include "FLOW.h"

//==================================================光流融合里程计========================================//
// 静态函数声明,以下函数均为该.c文件内部调用
static unsigned char Check_TOF400C(void);
static void TOF400C_Write_Datas(unsigned int reg, unsigned char *dat, unsigned long num);
static void TOF400C_Read_Datas(unsigned int reg, unsigned char *dat, unsigned long num);
static void TOF400C_Write_Data(unsigned int reg, unsigned char dat);
static void TOF400C_Read_Data(unsigned int  reg, unsigned char *dat);
static void TOF400C_Write_Word_Data(unsigned int reg, unsigned int dat);
static void TOF400C_Read_Word_Data(unsigned int reg, unsigned int *dat);
// TOF400C配置参数
const unsigned char VL51L1X_CONFIGURATION[] =
{
	0x29,0x02,0x10,0x00,0x25,0xbc,0x51,0x81,	// 0`7
	0x09,0x07,0x93,0x00,0xff,0xff,0x75,0xff, 	// 8`15
	0xfe,0x0d,0x00,0x17,0x01,0x00,0x00,0x00, 	// 16`23
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, 	// 24`31
	0x30,0x00,0x17,0x0a,0x00,0x00,0x00,0x00, 	// 32`39
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x11, 	// 40`47
	0x02,0x00,0x02,0x08,0x00,0x08,0x10,0x01, 	// 48`55
	0x01,0x00,0x00,0x00,0x00,0xff,0x00,0x02, 	// 56`63
	0x00,0x00,0x00,0x00,0x00,0x20,0x0b,0x00, 	// 64`71
	0x00,0x02,0xff,0x21,0x00,0x00,0x01,0x00, 	// 72`79
	0x00,0x00,0x00,0x8c,0x00,0x00,0x38,0xff, 	// 80`87
	0x01,0x00,0x27,0x00,0x34,0x00,0x65,0x07, 	// 88`95
	0x00,0x87,0x05,0x01,0x68,0x00,0xc0,0x08, 	// 96`103
	0x38,0x00,0x00,0x00,0x00,0x99,0xc8,0x00, 	// 104`111
	0x03,0x00,0x00,0x00,0x00,0x00,0x02,0x07, 	// 112`119
	0x05,0x06,0x06,0x03,0x00,0x02,0xc7,0xff, 	// 120`127
	0xdb,0x02,0x00,0x00,0x01,0x01,0x21,				// 128`134
};
/**
TOF400C模块初始化
**/
void TOF400C_init(void)
{
		 unsigned char tmp = 0;
    // 初始化超时
    char time = TOF400C_TIMEOUT_COUNT;
	  TOF400C_SCL_LEVEL(0);
    TOF400C_SDA_LEVEL(0);
		while(tmp == 0)
		{
				    TOF400C_Write_Datas(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,VL51L1X_CONFIGURATION,135);
						TOF400C_Write_Word_Data(RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x0065);
						TOF400C_Write_Word_Data(RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x0087);	
				tmp = Check_TOF400C();
        time--;
        if(time < 0)
        {
					ips114_show_string(2,2,"TOF error!");
            while(1);
        }
				TOF400C_DELAY_MS(100);
		}
		TOF400C_Write_Data(SYSTEM__INTERRUPT_CLEAR, 0x21);
}
/**
获取TOF400C模块测量距离
**/

uint16 Get_Distance_TOF400C(void)
{
	unsigned char tmp;
		unsigned int tof400c_distance;
		TOF400C_Write_Data(SYSTEM__INTERRUPT_CLEAR, 0x01);
		TOF400C_Read_Data(RESULT__RANGE_STATUS, &tmp);
		if(tmp == 137)
		{
				TOF400C_Read_Word_Data(VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tof400c_distance);
		}
    return tof400c_distance;

}
/**
TOF400C获取中断极性
**/
static unsigned char Get_IntPol_TOF400C(void)
{
    unsigned char temp;
    unsigned char int_pol;
    TOF400C_Read_Data(GPIO_HV_MUX__CTRL, &temp);
    temp = temp & 0x10;
    int_pol = !(temp >> 4);
    return int_pol;
}

/**
TOF400C查看数据是否准备就绪,准备就绪返回1反之返回0
**/
static unsigned char Check_TOF400C(void)
{
    unsigned char temp;
    unsigned char int_pol;
    int_pol = Get_IntPol_TOF400C();
    TOF400C_Read_Data(GPIO__TIO_HV_STATUS, &temp);
    TOF400C_DELAY_MS(1);
    if ((temp & 1) == int_pol)
        return  1;
    else
        return  0;
}

/**
TOF400C IIC开始
**/
static void TOF400C_Start(void)
{
    TOF400C_SCL_LEVEL(1);
    TOF400C_SDA_LEVEL(1);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SDA_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SCL_LEVEL(0);
}

/**
TOF400C IIC停止
**/
static void TOF400C_Stop(void)
{
    TOF400C_SCL_LEVEL(0);
    TOF400C_SDA_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SCL_LEVEL(1);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SDA_LEVEL(1);
    TOF400C_DELAY_MS(TOF400C_DELAY);
}

/**
TOF400C 主机向从设备发送应答/非应答信号 1/0
**/
static void TOF400C_Sendack(unsigned char ack)
{
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    if(ack)
        TOF400C_SDA_LEVEL(0);
    else
        TOF400C_SDA_LEVEL(1);
    TOF400C_SCL_LEVEL(1);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
}

/**
TOF400C 主机等待从设备应答信号
**/
static int TOF400C_Waitack(void)
{
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SCL_LEVEL(1);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    if(TOF400C_SDA)
    {
        TOF400C_SCL_LEVEL(1);
        return 0;
    }
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    return 1;
}

/**
TOF400C 发送单个字节
**/
static void TOF400C_Write_Char(unsigned char ch)
{
    unsigned char i = 8;
    while(i--)
    {
        if(ch & 0x80)
            TOF400C_SDA_LEVEL(1);
        else
            TOF400C_SDA_LEVEL(0);
        ch <<= 1;
        TOF400C_DELAY_MS(TOF400C_DELAY);
        TOF400C_SCL_LEVEL(1);
        TOF400C_DELAY_MS(TOF400C_DELAY);
        TOF400C_SCL_LEVEL(0);
    }
    TOF400C_Waitack();
}

/**
TOF400C 接受一个字节数据
**/
static unsigned char TOF400C_Read_Char(unsigned char ack)
{
    unsigned char i;
    unsigned char c = 0;
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_SDA_LEVEL(1);
    for(i = 0; i < 8; i++)
    {
        TOF400C_DELAY_MS(TOF400C_DELAY);
        TOF400C_SCL_LEVEL(0);
        TOF400C_DELAY_MS(TOF400C_DELAY);
        TOF400C_SCL_LEVEL(1);
        TOF400C_DELAY_MS(TOF400C_DELAY);
        c <<= 1;
        if(TOF400C_SDA)
        {
            c += 1;
        }
    }
    TOF400C_SCL_LEVEL(0);
    TOF400C_DELAY_MS(TOF400C_DELAY);
    TOF400C_Sendack(ack);
    return c;
}

/**
TOF400C 连续向指定寄存器写多个字节数据
**/
static void TOF400C_Write_Datas(unsigned int reg, unsigned char *dat, unsigned long num)
{
    TOF400C_Start();
    TOF400C_Write_Char(VL53L1X_DEFAULT_DEVICE_ADDRESS);

    TOF400C_Write_Char((unsigned char)(reg >> 8));
    TOF400C_Write_Char(reg & 0x00ff);
    while(num--)
    {
        TOF400C_Write_Char(*dat++);
    }
    TOF400C_Stop();
}

/**
TOF400C 连续从指定寄存器读多个字节数据
**/
static void TOF400C_Read_Datas(unsigned int reg, unsigned char *dat, unsigned long num)
{
    unsigned char i = 0;
    TOF400C_Start();
    TOF400C_Write_Char(VL53L1X_DEFAULT_DEVICE_ADDRESS);

    TOF400C_Write_Char((unsigned char)(reg >> 8));
    TOF400C_Write_Char(reg & 0x00ff);

    TOF400C_Start();
    TOF400C_Write_Char(VL53L1X_DEFAULT_DEVICE_ADDRESS + 1);

    for(i = 0; i < num; i++)
    {
        if(i != (num - 1))
        {
            dat[i] = TOF400C_Read_Char(1);
        }
        else
        {
            dat[i] = TOF400C_Read_Char(0);
        }
    }
    TOF400C_Stop();
}

/**
TOF400C 向指定寄存器写入1个字节数据
**/
static void TOF400C_Write_Data(unsigned int reg, unsigned char dat)
{
    TOF400C_Write_Datas(reg, &dat, 1);
}

/**
TOF400C 从指定寄存器读取1个字节数据
**/
static void TOF400C_Read_Data(unsigned int  reg, unsigned char *dat)
{
    TOF400C_Read_Datas(reg, dat, 1);
}

/**
TOF400C 向指定寄存器写入2个字节数据
**/
static void TOF400C_Write_Word_Data(unsigned int reg, unsigned int dat)
{
    unsigned char buf[2];
    buf[0] = dat >> 8;
    buf[1] = dat & 0x00FF;
    TOF400C_Write_Datas(reg, (unsigned char *)buf, 2);
}

/**
TOF400C 从指定寄存器读取2个字节数据
**/
static void TOF400C_Read_Word_Data(unsigned int reg, unsigned int *dat)
{
    unsigned char buf[2] = {0, 0};
    TOF400C_Read_Datas(reg, buf, 2);
    *dat = (buf[0] << 8) + buf[1];
}

/**
PMW3901 spi向指定寄存器写入数据
**/
void PMW3901_write_register(spi_index_enum spi_n,uint8 register_name,uint8 dat){
register_name |= 0x80u;
PMW3901_CS(0);
spi_write_8bit_register(spi_n,register_name,dat);
PMW3901_CS(1);
}

/**
PMW3901 spi从指定寄存器读取数据
**/
uint8 PMW3901_read_register(spi_index_enum spi_n, uint8 register_name)
{
    uint8 dat = 0;
	  register_name &= ~0x80u;
    PMW3901_CS(0);
    dat = spi_read_8bit_register(spi_n,register_name);
    PMW3901_CS(1);
    return dat;
}

/**
PMW3901 按序软启动
**/
void PMW3901_initRegisters()
{  
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x00);
   PMW3901_write_register(PMW3901_SPI, 0x61, 0xAD);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x03);
   PMW3901_write_register(PMW3901_SPI, 0x40, 0x00);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x05);
   PMW3901_write_register(PMW3901_SPI, 0x41, 0xB3);
   PMW3901_write_register(PMW3901_SPI, 0x43, 0xF1);
   PMW3901_write_register(PMW3901_SPI, 0x45, 0x14);
   PMW3901_write_register(PMW3901_SPI, 0x5B, 0x32);
   PMW3901_write_register(PMW3901_SPI, 0x5F, 0x34);
   PMW3901_write_register(PMW3901_SPI, 0x7B, 0x08);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x06);
   PMW3901_write_register(PMW3901_SPI, 0x44, 0x1B);
   PMW3901_write_register(PMW3901_SPI, 0x40, 0xBF);
   PMW3901_write_register(PMW3901_SPI, 0x4E, 0x3F);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x08);
   PMW3901_write_register(PMW3901_SPI, 0x65, 0x20);
   PMW3901_write_register(PMW3901_SPI, 0x6A, 0x18);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x09);
   PMW3901_write_register(PMW3901_SPI, 0x4F, 0xAF);
   PMW3901_write_register(PMW3901_SPI, 0x5F, 0x40);
   PMW3901_write_register(PMW3901_SPI, 0x48, 0x80);
   PMW3901_write_register(PMW3901_SPI, 0x49, 0x80);
   PMW3901_write_register(PMW3901_SPI, 0x57, 0x77);
   PMW3901_write_register(PMW3901_SPI, 0x60, 0x78);
   PMW3901_write_register(PMW3901_SPI, 0x61, 0x78);
   PMW3901_write_register(PMW3901_SPI, 0x62, 0x08);
   PMW3901_write_register(PMW3901_SPI, 0x63, 0x50);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x0A);
   PMW3901_write_register(PMW3901_SPI, 0x45, 0x60);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x00);
   PMW3901_write_register(PMW3901_SPI, 0x4D, 0x11);
   PMW3901_write_register(PMW3901_SPI, 0x55, 0x80);
   PMW3901_write_register(PMW3901_SPI, 0x74, 0x1F);
   PMW3901_write_register(PMW3901_SPI, 0x75, 0x1F);
   PMW3901_write_register(PMW3901_SPI, 0x4A, 0x78);
   PMW3901_write_register(PMW3901_SPI, 0x4B, 0x78);
   PMW3901_write_register(PMW3901_SPI, 0x44, 0x08);
   PMW3901_write_register(PMW3901_SPI, 0x45, 0x50);
   PMW3901_write_register(PMW3901_SPI, 0x64, 0xFF);
   PMW3901_write_register(PMW3901_SPI, 0x65, 0x1F);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x14);
   PMW3901_write_register(PMW3901_SPI, 0x65, 0x60);
   PMW3901_write_register(PMW3901_SPI, 0x66, 0x08);
   PMW3901_write_register(PMW3901_SPI, 0x63, 0x78);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x15);
   PMW3901_write_register(PMW3901_SPI, 0x48, 0x58);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x07);
   PMW3901_write_register(PMW3901_SPI, 0x41, 0x0D);
   PMW3901_write_register(PMW3901_SPI, 0x43, 0x14);
   PMW3901_write_register(PMW3901_SPI, 0x4B, 0x0E);
   PMW3901_write_register(PMW3901_SPI, 0x45, 0x0F);
   PMW3901_write_register(PMW3901_SPI, 0x44, 0x42);
   PMW3901_write_register(PMW3901_SPI, 0x4C, 0x80);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x10);
   PMW3901_write_register(PMW3901_SPI, 0x5B, 0x02);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x07);
   PMW3901_write_register(PMW3901_SPI, 0x40, 0x41);
   PMW3901_write_register(PMW3901_SPI, 0x70, 0x00);
   system_delay_ms(10);
   PMW3901_write_register(PMW3901_SPI, 0x32, 0x44);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x07);
   PMW3901_write_register(PMW3901_SPI, 0x40, 0x40);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x06);
   PMW3901_write_register(PMW3901_SPI, 0x62, 0xF0);
   PMW3901_write_register(PMW3901_SPI, 0x63, 0x00);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x0D);
   PMW3901_write_register(PMW3901_SPI, 0x48, 0xC0);
   PMW3901_write_register(PMW3901_SPI, 0x6F, 0xD5);
   PMW3901_write_register(PMW3901_SPI, 0x7F, 0x00);
   PMW3901_write_register(PMW3901_SPI, 0x5B, 0xA0);
   PMW3901_write_register(PMW3901_SPI, 0x4E, 0xA8);
   PMW3901_write_register(PMW3901_SPI, 0x5A, 0x50);
   PMW3901_write_register(PMW3901_SPI, 0x40, 0x80);
}
/**
PMW3901模块初始化
**/
void PMW3901_init(){
spi_init(PMW3901_SPI,0,PMW3901_SPI_SPEED,PMW3901_CLK_PIN,PMW3901_MOS_PIN,PMW3901_MIS_PIN,SPI_CS_NULL); 
gpio_init(PMW3901_CS_PIN,GPO,GPIO_HIGH,GPO_PUSH_PULL);  
system_delay_ms(200);
PMW3901_write_register(PMW3901_SPI,0x3A,0x5A);//复位
system_delay_ms(100);
PMW3901_read_register(PMW3901_SPI,0x02);
PMW3901_read_register(PMW3901_SPI,0x03);
PMW3901_read_register(PMW3901_SPI,0x04);
PMW3901_read_register(PMW3901_SPI,0x05);
PMW3901_read_register(PMW3901_SPI,0x06);
system_delay_ms(500);
PMW3901_initRegisters();
system_delay_ms(500);
}
/**
PMW3901模块读取原始数据和累计位移
**/
void readMotionCount(int16 *deltax, int16 *deltay){
PMW3901_read_register(PMW3901_SPI,0x02);
*deltax = ((int16)PMW3901_read_register(PMW3901_SPI,0x04) << 8) |PMW3901_read_register(PMW3901_SPI,0x03);
*deltay = ((int16)PMW3901_read_register(PMW3901_SPI,0x06) << 8) | PMW3901_read_register(PMW3901_SPI,0x05);
}

/**
光流计初始化
**/
void FLOW_init(){
system_delay_ms(10);
PMW3901_init();
system_delay_ms(10);
TOF400C_init();
}