#include "lidar.h"

volatile uint8_t usart3_index = 0;
volatile uint8_t usart3_checksum = 0;
volatile uint8_t usart3_frame_ready = 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);
}

uint32_t test;
void USART3_IRQHandler()
{
	++test;
	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 (!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;
}