#include <libopencm3/stm32/f1/rcc.h>
#include <libopencm3/stm32/f1/gpio.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/stm32/nvic.h>
#include <libopencm3/stm32/usart.h>

#include <stdio.h>
#include <errno.h>

int pulse_width_abs = 130;
int pulse_width_speed_div = 15;

int min_speed = 500;
int max_speed = 2000;
int accel = 800;

/* sinewave in degrees */
int8_t sinewave[360] = {0, 2, 4, 6, 8, 11, 13, 15, 17, 19, 22, 24, 26, 28, 30, 32, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 72, 74, 76, 78, 79, 81, 83, 84, 86, 88, 89, 91, 92, 94, 95, 97, 98, 100, 101, 102, 104, 105, 106, 107, 108, 109, 111, 112, 113, 114, 115, 116, 116, 117, 118, 119, 120, 120, 121, 122, 122, 123, 123, 124, 124, 125, 125, 125, 126, 126, 126, 126, 126, 126, 126, 127, 126, 126, 126, 126, 126, 126, 126, 125, 125, 125, 124, 124, 123, 123, 122, 122, 121, 120, 120, 119, 118, 117, 116, 116, 115, 114, 113, 112, 111, 109, 108, 107, 106, 105, 104, 102, 101, 100, 98, 97, 95, 94, 92, 91, 89, 88, 86, 84, 83, 81, 79, 78, 76, 74, 72, 71, 69, 67, 65, 63, 61, 59, 57, 55, 53, 51, 49, 47, 45, 43, 41, 39, 37, 35, 32, 30, 28, 26, 24, 22, 19, 17, 15, 13, 11, 8, 6, 4, 2, 0, -2, -4, -6, -8, -11, -13, -15, -17, -19, -22, -24, -26, -28, -30, -32, -35, -37, -39, -41, -43, -45, -47, -49, -51, -53, -55, -57, -59, -61, -63, -65, -67, -69, -71, -72, -74, -76, -78, -79, -81, -83, -84, -86, -88, -89, -91, -92, -94, -95, -97, -98, -100, -101, -102, -104, -105, -106, -107, -108, -109, -111, -112, -113, -114, -115, -116, -116, -117, -118, -119, -120, -120, -121, -122, -122, -123, -123, -124, -124, -125, -125, -125, -126, -126, -126, -126, -126, -126, -126, -127, -126, -126, -126, -126, -126, -126, -126, -125, -125, -125, -124, -124, -123, -123, -122, -122, -121, -120, -120, -119, -118, -117, -116, -116, -115, -114, -113, -112, -111, -109, -108, -107, -106, -105, -104, -102, -101, -100, -98, -97, -95, -94, -92, -91, -89, -88, -86, -84, -83, -81, -79, -78, -76, -74, -72, -71, -69, -67, -65, -63, -61, -59, -57, -55, -53, -51, -49, -47, -45, -43, -41, -39, -37, -35, -32, -30, -28, -26, -24, -22, -19, -17, -15, -13, -11, -8, -6, -4, -2};

const int pwm_freq = 56250; /* 56.25 kHz */

void usart_setup(void)
{
	rcc_peripheral_enable_clock(&RCC_APB2ENR,
		RCC_APB2ENR_IOPAEN | RCC_APB2ENR_AFIOEN | RCC_APB2ENR_USART1EN);

	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9);

	usart_set_baudrate(USART1, 9600);
	usart_set_databits(USART1, 8);

	usart_set_stopbits(USART1, USART_STOPBITS_1);
	usart_set_mode(USART1, USART_MODE_TX_RX);
	usart_set_parity(USART1, USART_PARITY_NONE);
	usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE);

	usart_enable(USART1);
}

int _write(int file, char *ptr, int len)
{
	int i;

	if (file == 1) {
		for (i = 0; i < len; i++) {
			usart_send_blocking(USART1, ptr[i]);
		}

		return i;
	}

	errno = EIO;
	return -1;
}

int _read(int file, char *ptr, int len)
{
	int i;

	if (file == 0) {
		for (i = 0; i < len; i++) {
			*ptr++ = usart_recv_blocking(USART1);
		}

		return i;
	}

	errno = EIO;
	return -1;
}

uint32_t *phase_reg[4];
int phases_c = sizeof(phase_reg) / sizeof(uint32_t *);

void set_angle(unsigned int deg, unsigned int pulse_width)
{
	unsigned int i;
	for (i = 0; i < phases_c; i++) {
		int16_t val = sinewave[(deg + i * (360 / phases_c)) % 360];
		val *= pulse_width;
		val /= 255;
		*(phase_reg[i]) = (val > 0 ? val : 0);
	}
}

int pulse_width;

int counter = 0;
int last_change = 0;

int stop = 0;

int speed = 0;

int pos = 0;
int tgt_pos = 0;

int curr_accel = 0;
int change = 0;

int calc_speed()
{
	int dir = pos < tgt_pos ? 1 : -1;

	unsigned int abs_speed = speed > 0 ? speed : -speed;

	if (pos == tgt_pos && abs_speed <= min_speed) {
		last_change = counter;
		return 0;
	}

	curr_accel = 0;

	/* FIXME: buggy */

	if (speed * dir < 0) {
		curr_accel = accel * dir;
	} else if (abs_speed * abs_speed >= ((tgt_pos - pos) * dir - 1) * accel * 2) {
		curr_accel = accel * -dir;
	} else {
		if (abs_speed < max_speed)
			curr_accel = accel * dir;

		if (abs_speed > max_speed)
			curr_accel = accel * -dir;
	}

	if (curr_accel) {
		change = curr_accel * (counter - last_change) / pwm_freq;
		last_change += change * pwm_freq / curr_accel;

		return speed + change;
	} else {
		last_change = counter;

		return speed;
	}
}

void tim4_isr()
{
	if (stop) {
		TIM4_CCR1 = 0;
		TIM4_CCR2 = 0;
		TIM4_CCR3 = 0;
		TIM4_CCR4 = 0;
		TIM_SR(TIM4) &= ~TIM_SR_UIF;
		return;
	}

	counter++;

	int abs_speed = speed > 0 ? speed : -speed;
	
	if (speed) {
		if (counter % (pwm_freq / abs_speed) == 0) {
			pos += speed > 0 ? 1 : -1;
		}
	}

	if (counter % (pwm_freq / 1000) == 0) {
		speed = calc_speed();
	}

	pulse_width = pulse_width_abs + abs_speed / pulse_width_speed_div;

	/* TODO */
	set_angle((pos + 100000) % 360, pulse_width < 255 ? pulse_width : 255);

	TIM_SR(TIM4) &= ~TIM_SR_UIF;
}

void set_tgt_pos(int pos)
{
	tgt_pos = pos;
}

void timer_setup()
{
	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
	rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN);

	gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
		GPIO6 | GPIO7 | GPIO8 | GPIO9);

	/* 72MHz / 10 = 7.2MHz */
	TIM_PSC(TIM4) = 9;

	/* 7.2MHz / 128 = 56.250kHz */
	TIM_CNT(TIM4) = 0;
	TIM_ARR(TIM4) = 128;

	TIM_EGR(TIM4) = TIM_EGR_UG;

	TIM_CCMR1(TIM4) = TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC1PE
		| TIM_CCMR1_OC2M_PWM1 | TIM_CCMR1_OC2PE;
	TIM_CCMR2(TIM4) = TIM_CCMR2_OC3M_PWM1 | TIM_CCMR2_OC3PE
		| TIM_CCMR2_OC4M_PWM1 | TIM_CCMR2_OC4PE;

	TIM_CCER(TIM4) = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E;

	TIM_CCR1(TIM4) = 0;
	TIM_CCR2(TIM4) = 0;
	TIM_CCR3(TIM4) = 0;
	TIM_CCR4(TIM4) = 0;

	TIM_DIER(TIM4) = TIM_DIER_UIE;
	nvic_enable_irq(NVIC_TIM4_IRQ);
	nvic_set_priority(NVIC_TIM4_IRQ, 1);

	TIM_CR1(TIM4) |= TIM_CR1_CEN;
}

int main(void)
{
	int i;

	setvbuf(stdin, NULL, _IONBF, 0);
	setvbuf(stdout, NULL, _IONBF, 0);

	rcc_clock_setup_in_hse_8mhz_out_72mhz();

	phase_reg[0] = &TIM4_CCR1;
	phase_reg[1] = &TIM4_CCR2;
	phase_reg[2] = &TIM4_CCR3;
	phase_reg[3] = &TIM4_CCR4;

	speed = 0;
	set_tgt_pos(0);

	usart_setup();
	timer_setup();

	char c;
	while (1) {
		c = getchar();

		switch(c) {
			case 's':
				stop = 1;
				break;
			case 'r':
				printf("current position is %d\r\n", pos);
				printf("enter target position: ");
				scanf("%d", &tgt_pos);
				printf("\r\n");
				printf("target position is %d\r\n", tgt_pos);
				set_tgt_pos(tgt_pos);
				break;
			case 'v':
				printf("current pulse_width_abs is %d\r\n", pulse_width_abs);
				printf("enter new pulse_width_abs: ");
				scanf("%d", &pulse_width_abs);
				printf("\r\n");
				break;
			case 'b':
				printf("current pulse_width_speed_div is %d\r\n", pulse_width_speed_div);
				printf("enter new pulse_width_speed_div: ");
				scanf("%d", &pulse_width_speed_div);
				printf("\r\n");
				break;
			case 'n':
				printf("current min_speed is %d\r\n", min_speed);
				printf("enter new min_speed: ");
				scanf("%d", &min_speed);
				printf("\r\n");
				break;
			case 'm':
				printf("current max_speed is %d\r\n", max_speed);
				printf("enter new max_speed: ");
				scanf("%d", &max_speed);
				printf("\r\n");
				break;
			default:
				printf("pos: %d tgt_pos: %d speed: %d pulse_width: %d curr_accel: %d change: %d last_change: %d\r\n",
					pos, tgt_pos, speed, pulse_width, curr_accel, change, last_change);
		}
	}

	return 0;
}