/*
 * main.c
 *
 *  Created on: Aug 2, 2016
 *      Author: tkl
 */

#include <stdbool.h>
#include <string.h>
#include <stdio.h>

#include "driver.h"
#include "board.h"
#include "stack.h"
#include "queue.h"
#include "kernel.h"
#include "driver.h"
#include "list.h"
#include "shell.h"

static TIM_HandleTypeDef timer_handle = {
		.Instance = TIM2,
		.Init.Prescaler = 1000,
		.Init.CounterMode = TIM_COUNTERMODE_UP,
		.Init.Period = 0xffff,
		.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1,
		.Init.RepetitionCounter = 0,
};

static TIM_IC_InitTypeDef input_capture_init = {
		.ICPrescaler = TIM_ICPSC_DIV1,
		.ICFilter = 0,
//		.ICPolarity = TIM_ICPOLARITY_FALLING,
		.ICPolarity = TIM_ICPOLARITY_BOTHEDGE,
		.ICSelection = TIM_ICSELECTION_INDIRECTTI,
};

static TIM_SlaveConfigTypeDef   slave_config = {
		.SlaveMode = TIM_SLAVEMODE_RESET,
		.InputTrigger = TIM_TS_TI1FP1,
		.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE,
		.TriggerFilter = 0,
};

static GPIO_InitTypeDef port_cfg_b10 = {
		.Pin = GPIO_PIN_10,
		.Mode = GPIO_MODE_AF_OD,
		.Speed = GPIO_SPEED_FREQ_HIGH,
		.Pull = GPIO_NOPULL,
		.Alternate = GPIO_AF1_TIM2,
};

static const struct stm32f4_gpio b10_gpio = {
		.port = GPIOB,
		.pin = &port_cfg_b10,
};

static const struct gpio __gpio_b10 = {
		(void*)&b10_gpio,
		&gpio_fp
};

static const struct driver gpio_b10 = {
		DRIVER_TYPE_GPIO,
		&__gpio_b10,
};

static int period = 0, period_start = 0, pulse = 0, pulse_start = 0;

void config_input(void)
{
	__HAL_RCC_TIM2_CLK_ENABLE();
	drv_open(&gpio_b10);
	HAL_NVIC_SetPriority(TIM2_IRQn, 0, 1);

	/* cfg timer */
	HAL_TIM_IC_Init(&timer_handle);
	HAL_TIM_IC_ConfigChannel(&timer_handle, &input_capture_init, TIM_CHANNEL_4);
	HAL_TIM_SlaveConfigSynchronization(&timer_handle, &slave_config);
	HAL_TIM_IC_Start_IT(&timer_handle, TIM_CHANNEL_4);

	HAL_NVIC_EnableIRQ(TIM2_IRQn);
}

int main(void)
{
	char print_buffer[80];
	drv_open(&pwm_1);
	drv_open(&pwm_2);
	drv_open(&pwm_3);
	drv_open(&pwm_4);

	drv_open(&pwm5_c2);

	drv_open(&uart_1);

	config_input();

	while(1) {
		for(unsigned int duty = 0; duty < 100; duty++) {
			drv_ioctl(&pwm_1, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_2, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_3, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			sleep_ms(100);

			float dist = 84000.0 / pulse;
			dist = 343.0 / dist;
			dist /= 2.0;
			dist *= 100; // m -> cm
			int d = (int)dist;
			sprintf(print_buffer, "distance: %d cm\r\n", d);
			drv_write(&uart_1, print_buffer, strlen(print_buffer));
		}
		for(unsigned int duty = 98; duty > 0; duty--) {
			drv_ioctl(&pwm_1, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_2, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_3, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			sleep_ms(100);

			float dist = 84000.0 / pulse;
			dist = 343.0 / dist;
			dist /= 2.0;
			dist *= 100; // m -> cm
			int d = (int)dist;
			sprintf(print_buffer, "distance: %d cm\r\n", d);
			drv_write(&uart_1, print_buffer, strlen(print_buffer));
		}
	}

	return 0;
}

#include "isr.h"
void TIM2_IRQHandler(void)
{
	enter_isr();
	TIM_HandleTypeDef *htim = &timer_handle;

	/* Capture compare 4 event */
	if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC4) != RESET) {
		if(__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC4) !=RESET) {
			__HAL_TIM_CLEAR_IT(htim, TIM_IT_CC4);
			/* Input capture event */
			if((htim->Instance->CCMR2 & TIM_CCMR2_CC4S) != 0x00U) {
				uint32_t ch4 = htim->Instance->CCR4;
				if(GPIO_PIN_SET == HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_10)) {
					if(ch4 > period_start)
						period = ch4 - period_start;
					period_start = ch4;
					pulse_start = ch4;
				}
				else {
					if(ch4 > pulse_start)
						pulse = ch4 - pulse_start;
				}
			}
		}
	}

	exit_isr();
}