#include "lidar.h"

volatile uint8_t usart3_index = 0;
volatile uint8_t usart3_checksum = 0;
volatile uint8_t usart3_frame_ready = 0;
volatile uint8_t lidar_update_flag = 0;
static lidar_data_buf buffer;
static uint8_t* buff_data = (uint8_t*)&buffer;

void lidar_init()
{
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
	
	// port 11 alt func mode
	GPIOB->MODER &= ~(3 << (11 * 2));
	GPIOB->MODER |= 2 << (11 * 2);
	
	// set AF7 on AFRegister for GPIOB11
	GPIOB->AFR[1] &= ~(0xF << 12);
	GPIOB->AFR[1] |= 7 << 12;
	
	// very high speed
	GPIOB->OSPEEDR |= 3 << (11 * 2);
	
	// pull-up
	GPIOB->PUPDR &= ~(3 << (11 * 2));
	GPIOB->PUPDR |= 1 << (11 * 2);
	
	// SYSCLK selected as USART3 clock
	RCC->CCIPR &= ~(RCC_CCIPR_USART3SEL);
	RCC->CCIPR |= 1 << RCC_CCIPR_USART3SEL_Pos;
	RCC->APB1ENR1 |= RCC_APB1ENR1_USART3EN;
	
	USART3->CR1 = 0;
	USART3->CR2 = 0;
	USART3->CR3 = 0;
	
	USART3->BRR = 16000000UL / 115200UL;
	
	// parity control disable
	USART3->CR1 &= ~USART_CR1_PCE;
	
	// word length 8 bit
	USART3->CR1 &= ~USART_CR1_M1 & ~USART_CR1_M0;
	
	// 1 stop bit
	USART3->CR2 &= ~USART_CR2_STOP;
	
	// receiver enable
	// interrupt generated whenever ORE = 1 or RXNE = 1
	USART3->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
	
	// overrun disable
	// USART3->CR3 |= USART_CR3_OVRDIS;
	
	// USART3 enable
	USART3->CR1 |= USART_CR1_UE;
	
	// Interrupt enable
	NVIC_EnableIRQ(USART3_IRQn);
}

void lidar_tim7_init()
{
	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
	
	TIM7->PSC = 16000 - 1;        // 16 MHz / 16000 = 1000 Hz (1 ms)
	TIM7->ARR = 10 - 1;           // 10 ms
	
	TIM7->DIER |= TIM_DIER_UIE;   // interrupt enable
	TIM7->CR1 |= TIM_CR1_CEN;     // counter enable
	
	NVIC_EnableIRQ(TIM7_DAC_IRQn);
}
	
void TIM7_DAC_IRQHandler()
{
	if (TIM7->SR & TIM_SR_UIF)
	{
		TIM7->SR &= ~TIM_SR_UIF;
		lidar_update_flag = 1;
	}
}
	
void USART3_IRQHandler()
{
	if (USART3->ISR & USART_ISR_RXNE)
	{
		uint8_t b = USART3->RDR;
		
		if (usart3_index < 2)
		{
			if (b == USART3_START_BYTE)
				buff_data[usart3_index++] = b;
		}
		else if (usart3_index < USART3_FRAME_SIZE)
			buff_data[usart3_index++] = b;
		
		if (usart3_index == USART3_FRAME_SIZE)
		{
			usart3_index = 0;
			usart3_frame_ready = 1;
		}
	}
}

void lidar_update(lidar_data* lidar)
{
	if (!lidar_update_flag)
		return;
	
	if (!usart3_frame_ready)
		return;
	
	usart3_frame_ready = 0;
	
	for (uint8_t i = 0; i < USART3_FRAME_SIZE; ++i) usart3_checksum += buff_data[i];
	
	if (buffer.checksum != usart3_checksum)
	  return;
	
	lidar->distance		= 	buffer.distance_l 	| (buffer.distance_h << 8);
	lidar->strength		= 	buffer.strength_l 	| (buffer.strength_h << 8);
	lidar->temperature	= 	buffer.temp_l 		| (buffer.temp_h << 8);
}

void lidar_i2c2_init()
{
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
	
	GPIOA->MODER &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
	GPIOA->MODER |= 2 << (8 * 2) | 2 << (9 * 2);		// alt func mode
	
	GPIOA->AFR[1] &= ~(0xF << 0) & ~(0xF << 4);
	GPIOA->AFR[1] |= 4 << 0 | 4 << 4;					// AF4
	
	GPIOA->OTYPER |= 1 << 8 | 1 << 9;					// open-drain
	
	GPIOA->PUPDR &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
	GPIOA->PUPDR |= 1 << (8 * 2) | 1 << (9 * 2);		// pull-up
	
	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C2EN;				// enable I2C2
	I2C2->TIMINGR = 0x00303D5B;							// 400 kHz @ 16 MHz
	I2C2->CR1 |= I2C_CR1_PE;
}

static void i2c2_wait_txis()
{
    while (!(I2C2->ISR & I2C_ISR_TXIS));
}

static void i2c2_wait_stop()
{
    while (!(I2C2->ISR & I2C_ISR_STOPF));
    I2C2->ICR |= I2C_ICR_STOPCF;
}

static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size)
{
    while (I2C2->ISR & I2C_ISR_BUSY);

    I2C2->CR2 = 0;
    I2C2->CR2 |= (addr << 1);                 // 7-bit addr
    I2C2->CR2 |= (size << 16);                // bite count
    I2C2->CR2 |= I2C_CR2_AUTOEND;             // auto stop
    I2C2->CR2 |= I2C_CR2_START;               // start

    for (uint8_t i = 0; i < size; i++)
    {
        i2c2_wait_txis();
        I2C2->TXDR = data[i];
    }

    i2c2_wait_stop();

    // check NACK
    if (I2C2->ISR & I2C_ISR_NACKF)
    {
        I2C2->ICR |= I2C_ICR_NACKCF;
        return 0;
    }

    return 1;
}

void tf02_force_uart()
{
    uint8_t cmd_uart[] = {0x5A, 0x05, 0x0A, 0x00, 0x69};
    uint8_t cmd_save[] = {0x5A, 0x04, 0x11, 0x6F};

    // force UART command
    if (!i2c2_write(TF02_I2C_ADDR, cmd_uart, sizeof(cmd_uart)))
    {
        // no ACK — lidar is not on i2c
        return;
    }

    for (volatile int i = 0; i < 100000; i++);

	// save command
    i2c2_write(TF02_I2C_ADDR, cmd_save, sizeof(cmd_save));

    for (volatile int i = 0; i < 200000; i++);
}