#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/adc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/dma.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/systick.h>

#include <fxp.h>
#include <fxp_basic.h>

#include "debug.h"

volatile int wait_frame = 1;

#define ADC_NUM_CHANNELS 3
volatile uint16_t adc_values[ADC_NUM_CHANNELS];

#define LCD_USE_4BIT_MODE 1

#define LCD_PORT    GPIOA
#define LCD_E       GPIO10
#define LCD_RW      GPIO11
#define LCD_RS      GPIO12
#define LCD_D4      GPIO4
#define LCD_D5      GPIO5
#define LCD_D6      GPIO6
#define LCD_D7      GPIO7

enum LCDRegType {
	LCD_REG_CONTROL,
	LCD_REG_DATA
};

struct LCDCommand {
	uint8_t         data;
	enum LCDRegType reg_type;
};

#define LCD_QUEUE_LEN 64
struct LCDCommand lcd_cmd_buffer[LCD_QUEUE_LEN];
uint16_t lcd_buf_read_pos, lcd_buf_write_pos;

static inline void busy_wait(uint32_t cycles)
{
	for (uint32_t i = 0; i < cycles; i++) {
		__asm__("nop");
	}
}

#if LCD_USE_4BIT_MODE
/*
 * Send 4 bits to the LCD. Only use directly during initialization.
 *
 * The display samples data on the falling edge of E.
 *
 * \param data    lower nibble contains the data to send
 */
static void lcd_send_4bit(uint8_t data, enum LCDRegType reg_type)
{
	uint8_t gpios2set = 0, gpios2clear = 0;

	// set E high (rising edge has no effect)
	gpio_set(LCD_PORT, LCD_E);

	if(reg_type == LCD_REG_CONTROL) {
		gpio_clear(LCD_PORT, LCD_RS);
	} else { // LCD_REG_DATA
		gpio_set(LCD_PORT, LCD_RS);
	}

	busy_wait(100);

	// calculate target gpio states
	if(data & (1 << 0)) { gpios2set |= LCD_D4; } else { gpios2clear |= LCD_D4; }
	if(data & (1 << 1)) { gpios2set |= LCD_D5; } else { gpios2clear |= LCD_D5; }
	if(data & (1 << 2)) { gpios2set |= LCD_D6; } else { gpios2clear |= LCD_D6; }
	if(data & (1 << 3)) { gpios2set |= LCD_D7; } else { gpios2clear |= LCD_D7; }

	// apply data pins
	gpio_set(LCD_PORT, gpios2set);
	gpio_clear(LCD_PORT, gpios2clear);

	busy_wait(100);

	// set E low (display samples on falling edge)
	gpio_clear(LCD_PORT, LCD_E);

	busy_wait(100);
}
#else /* !LCD_USE_4BIT_MODE */
static void lcd_send_8bit(uint8_t data, enum LCDRegType reg_type)
{
	uint32_t gpios2set = 0, gpios2clear = 0;

	// set E high (rising edge has no effect)
	gpio_set(LCD_PORT, LCD_E);

	if(reg_type == LCD_REG_CONTROL) {
		gpio_clear(LCD_PORT, LCD_RS);
	} else { // LCD_REG_DATA
		gpio_set(LCD_PORT, LCD_RS);
	}

	busy_wait(100);

	// calculate target gpio states
	if(data & (1 << 0)) { gpios2set |= LCD_D0; } else { gpios2clear |= LCD_D0; }
	if(data & (1 << 1)) { gpios2set |= LCD_D1; } else { gpios2clear |= LCD_D1; }
	if(data & (1 << 2)) { gpios2set |= LCD_D2; } else { gpios2clear |= LCD_D2; }
	if(data & (1 << 3)) { gpios2set |= LCD_D3; } else { gpios2clear |= LCD_D3; }
	if(data & (1 << 4)) { gpios2set |= LCD_D4; } else { gpios2clear |= LCD_D4; }
	if(data & (1 << 5)) { gpios2set |= LCD_D5; } else { gpios2clear |= LCD_D5; }
	if(data & (1 << 6)) { gpios2set |= LCD_D6; } else { gpios2clear |= LCD_D6; }
	if(data & (1 << 7)) { gpios2set |= LCD_D7; } else { gpios2clear |= LCD_D7; }

	// apply data pins
	gpio_set(LCD_PORT, gpios2set);
	gpio_clear(LCD_PORT, gpios2clear);

	busy_wait(100);

	// set E low (display samples on falling edge)
	gpio_clear(LCD_PORT, LCD_E);

	busy_wait(100);
}
#endif /* LCD_USE_4BIT_MODE */

static void lcd_send_init(uint8_t data, enum LCDRegType reg_type)
{
#if LCD_USE_4BIT_MODE
	lcd_send_4bit((data >> 4) & 0x0F, reg_type);
#else
	lcd_send_8bit(data, reg_type);
#endif
}

static void lcd_init(void)
{
	lcd_buf_write_pos = 0;
	lcd_buf_read_pos = 0;
}

static void lcd_send(uint8_t data, enum LCDRegType reg_type)
{
#if LCD_USE_4BIT_MODE
	lcd_send_4bit((data >> 4) & 0x0F, reg_type);
	lcd_send_4bit((data >> 0) & 0x0F, reg_type);
#else
	lcd_send_8bit(data, reg_type);
#endif
}

/*!
 * Send next command from the queue.
 */
static int lcd_process(void)
{
	if(lcd_buf_read_pos != lcd_buf_write_pos) {
		lcd_send(lcd_cmd_buffer[lcd_buf_read_pos].data,
		         lcd_cmd_buffer[lcd_buf_read_pos].reg_type);

		lcd_buf_read_pos++;
		if(lcd_buf_read_pos >= LCD_QUEUE_LEN) {
			lcd_buf_read_pos = 0;
		}

		return 0; // success
	} else {
		return -1; // queue empty
	}
}

static int lcd_enqueue(uint8_t data, enum LCDRegType reg_type)
{
	uint16_t tmp_pos = lcd_buf_write_pos + 1;
	if(tmp_pos >= LCD_QUEUE_LEN) {
		tmp_pos = 0;
	}

	if(tmp_pos != lcd_buf_read_pos) {
		lcd_cmd_buffer[tmp_pos].data = data;
		lcd_cmd_buffer[tmp_pos].reg_type = reg_type;

		lcd_buf_write_pos = tmp_pos;

		return 0; // success
	} else {
		return -1; // queue full
	}
}

static void lcd_send_string(char *data)
{
	while(*data) {
		lcd_enqueue((uint8_t)(*data), LCD_REG_DATA);
		data++;
	}
}

static void lcd_set_cursor_pos(uint8_t line, uint8_t col)
{
	lcd_enqueue(0x80 | (line << 6) | col, LCD_REG_CONTROL);
}


static void init_gpio(void)
{
	// Display GPIOs
	gpio_mode_setup(LCD_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,
			LCD_E | LCD_RW | LCD_RS | LCD_D4 | LCD_D5 | LCD_D6 | LCD_D7);

	// Set up UART TX on PB6 for debugging
	gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO6);
	gpio_set_af(GPIOB, GPIO_AF0, GPIO6);
}

static void init_clock(void)
{
	/* Set STM32 to 48 MHz. */
	// DANGER WARNING: ABP Clock is sysclk/2!
	// Relevant for Timers
	//rcc_clock_setup_in_hse_8mhz_out_48mhz();
	rcc_clock_setup_in_hsi_out_48mhz();

	// enable GPIO clocks:
	// Port B is needed for debugging
	rcc_periph_clock_enable(RCC_GPIOA);

	// Port A is needed for the Display and more
	rcc_periph_clock_enable(RCC_GPIOB);

	// enable TIM3 for scheduling
	rcc_periph_clock_enable(RCC_TIM3);

	// enable ADC1 clock
	rcc_periph_clock_enable(RCC_ADC1);

	// enable DMA
	rcc_periph_clock_enable(RCC_DMA);
}

static void init_timer(void)
{
#if 0
	// global interrupt config
	nvic_enable_irq(NVIC_TIM1_UP_IRQ);
	nvic_enable_irq(NVIC_TIM4_IRQ);

	// *** TIM1 ***
	// Generic scheduling

	// - upcounter
	// - clock: CK_INT
	// - only overflow generates update interrupt
	TIM1_CR1 |= TIM_CR1_URS;

	// defaults for TIM_CR2

	// enable update interrupt
	TIM1_DIER |= TIM_DIER_UIE;

	// prescaler
	TIM1_PSC = 71; // count up by 1 every 1 us

	// auto-reload (maximum value)
	TIM1_ARR = 999; // overflow every 1 ms

	// 48 kHz interrupt frequency
	// TIM1_PSC = 24; // count up by 1 every 208.33 ns
	// TIM1_ARR = 99; // multiply interval by 100 -> 20.833 us

	// GO!
	TIM1_CR1 |= TIM_CR1_CEN;

	// *** TIM4 ***
	// Configure channels 1 and 2 for PWM (-> Pins PB6, PB7)
	// used for the boost converter switching
	timer_reset(TIM4);

	timer_set_mode(TIM4, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);

	// set up PWM channels
	timer_set_oc_mode(TIM4, TIM_OC1, TIM_OCM_PWM1);
	timer_enable_oc_output(TIM4, TIM_OC1);
	timer_enable_oc_preload(TIM4, TIM_OC1);
	timer_set_oc_polarity_high(TIM4, TIM_OC1);

	timer_set_oc_mode(TIM4, TIM_OC2, TIM_OCM_PWM1);
	timer_enable_oc_output(TIM4, TIM_OC2);
	timer_enable_oc_preload(TIM4, TIM_OC2);
	timer_set_oc_polarity_low(TIM4, TIM_OC2);

	// prescaler
	timer_set_prescaler(TIM4, AUDIO_PWM_PRESCALER - 1);

	// auto-reload value
	timer_set_period(TIM4, AUDIO_PWM_PERIOD - 1); // -> PWM frequency = 120e6 / prescaler / period

	// enable update interrupt (triggered on timer restart)
	timer_enable_irq(TIM4, TIM_DIER_UIE);

	// GO!
	timer_enable_counter(TIM4);
#endif
	// *** TIM3 ***
	// used for the 1-millisecond system tick
	timer_reset(TIM3);

	timer_set_mode(TIM3, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);

	// prescaler
	timer_set_prescaler(TIM3, 47); // -> 1 us counting at 48 MHz

	// auto-reload value
	timer_set_period(TIM3, 999); // -> update interrupt every 1 ms

	// enable update interrupt (triggered on timer restart)
	timer_enable_irq(TIM3, TIM_DIER_UIE);
	nvic_enable_irq(NVIC_TIM3_IRQ);

	// GO!
	timer_enable_counter(TIM3);

}

static void init_adc(void)
{
#if 0
	//adc_set_multi_mode(ADC_CCR_MULTI_INDEPENDENT);
	
	adc_off(ADC2);

	adc_disable_scan_mode(ADC1);
	adc_set_single_conversion_mode(ADC1);
	adc_set_sample_time(ADC1, channel, ADC_SMPR_SMP_71DOT5CYC);
	adc_disable_external_trigger_regular(ADC1);
	adc_set_right_aligned(ADC1);
	adc_set_regular_sequence(ADC1, 1, &channel);
	adc_power_on(ADC1);

	channel = ADC_CHANNEL1;
	adc_disable_scan_mode(ADC2);
	adc_set_single_conversion_mode(ADC2);
	adc_set_sample_time(ADC2, channel, ADC_SMPR_SMP_71DOT5CYC);
	adc_disable_external_trigger_regular(ADC2);
	adc_set_right_aligned(ADC2);
	adc_set_regular_sequence(ADC2, 1, &channel);
	adc_power_on(ADC2);

	/* Wait for ADC starting up. */
	int i;
	for (i = 0; i < 800000; i++) /* Wait a bit. */
		__asm__("nop");

	adc_reset_calibration(ADC1);
	adc_calibration(ADC1);
	adc_reset_calibration(ADC2);
	adc_calibration(ADC2);
#endif
	uint8_t channels[ADC_NUM_CHANNELS] = {
		ADC_CHANNEL0, // VInSense
		ADC_CHANNEL1, // VOutSense
		ADC_CHANNEL2  // CurrentSense
	};

	adc_power_off(ADC1);

	// configure ADC
	//adc_enable_scan_mode(ADC1);
	adc_set_single_conversion_mode(ADC1);
	adc_set_resolution(ADC1, ADC_RESOLUTION_12BIT);
	adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_071DOT5);
	adc_disable_external_trigger_regular(ADC1);
	adc_set_right_aligned(ADC1);
	adc_set_regular_sequence(ADC1, ADC_NUM_CHANNELS, channels);

	// configure DMA for ADC

	//nvic_enable_irq(NVIC_DMA1_STREAM5_IRQ);
	dma_channel_reset(DMA1, DMA_CHANNEL1);
	dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_LOW);
	dma_set_memory_size(DMA1, DMA_CHANNEL1, DMA_CCR_MSIZE_16BIT);
	dma_set_peripheral_size(DMA1, DMA_CHANNEL1, DMA_CCR_PSIZE_16BIT);
	dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL1);
	dma_enable_circular_mode(DMA1, DMA_CHANNEL1);
	dma_set_read_from_peripheral(DMA1, DMA_CHANNEL1);
	dma_set_peripheral_address(DMA1, DMA_CHANNEL1, (uint32_t) &ADC1_DR);
	/* The array v[] is filled with the waveform data to be output */
	dma_set_memory_address(DMA1, DMA_CHANNEL1, (uint32_t) adc_values);
	dma_set_number_of_data(DMA1, DMA_CHANNEL1, ADC_NUM_CHANNELS);
	//dma_enable_transfer_complete_interrupt(DMA1, DMA_CHANNEL1);
	dma_enable_channel(DMA1, DMA_CHANNEL1);

	adc_enable_dma(ADC1);

	// GO!
	adc_power_on(ADC1);

}

#if 0
/* Set up timer to fire freq times per second */
static void init_systick(int freq)
{
	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
	/* clear counter so it starts right away */
	STK_CVR = 0;

	systick_set_reload(rcc_ahb_frequency / freq);
	systick_counter_enable();
	systick_interrupt_enable();
}
#endif


int main(void)
{
	uint16_t cpuload = 0;
	uint64_t timebase_ms = 0;
	char msg[128];
	char number[FXP_STR_MAXLEN];
	uint8_t sentSomething = 0;

	//fxp_t VIN_SCALE = fxp_from_float(3.3f * (100 + 12.4f) / 12.4f / 4095.0f);
	//fxp_t VOUT_SCALE = fxp_from_float(3.3f * (100 + 12.0f) / 12.0f / 4095.0f);
	fxp_t VIN_SCALE = fxp_from_float(12.11f / 1600.0f);
	fxp_t VOUT_SCALE = fxp_from_float(12.6f / 1620.0f);
	fxp_t CURRENT_SCALE = fxp_from_float(9.7f / 4095.0f);

	init_clock();
	init_gpio();
	init_adc();
	init_timer();

	lcd_init();
	debug_init();

	debug_send_string("Init complete\r\n");

	// boost converter PWM initial value
	//timer_set_oc_value(TIM3, TIM_OC1, 0);
	
	//init_systick(1000);

	// triggered every 1 ms
	while (1) {
		// let the ADC+DMA do its work
		adc_start_conversion_regular(ADC1);


		/*

		// *** Do some calculations while ADC converts ***

		// Ramp up target voltage
		if((targetVoltage < TARGET_VOLTAGE) && (voltRampUpCountdown-- == 0)) {
			targetVoltage += 2.0f;
			voltRampUpCountdown = VOLTAGE_UP_INTERVAL;

			if(targetVoltage > TARGET_VOLTAGE) {
				targetVoltage = TARGET_VOLTAGE;
			}
		}

		// read ADC value
		while(!adc_eoc(ADC1));
		adcval = adc_read_regular(ADC1);

		// scale current measurement
		curVoltage = VOLTAGE_MEAS_MAX * adcval / 4096.0f;

		// calculate error values
		pErr = targetVoltage - curVoltage;
		iErr += pErr;

		// limit integral error range
		if     (iErr >  IERR_LIMIT) iErr =  IERR_LIMIT;
		else if(iErr < -IERR_LIMIT) iErr = -IERR_LIMIT;

		// calculate the controller output ("action")
		controlAction = pErr * PGAIN + iErr * IGAIN;

		// calculate resulting PWM value
		if(controlAction > MAX_DUTY_CYCLE) {
			pwm_value = (int)(PWM_PERIOD * MAX_DUTY_CYCLE);
		} else if(controlAction > 0) {
			pwm_value = (int)(controlAction * PWM_PERIOD);
		} else {
			pwm_value = 0;
		}
		*/

		//timer_set_oc_value(TIM3, TIM_OC1, pwm_value);

		/*
		if((timebase_ms % 500) == 10) {
			sprintf(msg, "Audio PWM: %lu",
					dbg_audio_pwm_value);
			debug_send_string(msg);
			sentSomething = 1;
		}
		*/

		// wait for DMA transfer to complete
		while(!dma_get_interrupt_flag(DMA1, DMA_CHANNEL1, DMA_TCIF) && wait_frame);
		dma_clear_interrupt_flags(DMA1, DMA_CHANNEL1, DMA_TCIF);

		if((timebase_ms % 500) == 0) {
			debug_send_string("ADC:");

			for(uint8_t i = 0; i < ADC_NUM_CHANNELS; i++) {
				fxp_format_int(adc_values[i], msg);
				debug_send_string(" ");
				debug_send_string(msg);
			}

			fxp_t scaled_val;

			scaled_val = fxp_mult(fxp_from_int(adc_values[0]), VIN_SCALE);
			fxp_format(scaled_val, msg, 3);
			debug_send_string("\r\nInput[V]:   ");
			debug_send_string(msg);

			scaled_val = fxp_mult(fxp_from_int(adc_values[1]), VOUT_SCALE);
			fxp_format(scaled_val, msg, 3);
			debug_send_string("\r\nOutput[V]:  ");
			debug_send_string(msg);

			scaled_val = fxp_mult(fxp_from_int(adc_values[2]), CURRENT_SCALE);
			fxp_format(scaled_val, msg, 3);
			debug_send_string("\r\nCurrent[A]: ");
			debug_send_string(msg);

			sentSomething = 1;
		}

		if(sentSomething) {
			debug_send_string("\r\n");
			sentSomething = 0;
		}

		/* Display INIT:
		 * Power On
		 * sleep(15) minimum
		 * send(0x30, RW = RS = 0) -> command = 0x3X
		 * sleep(4.1) minimum
		 * send(0x30, RW = RS = 0) -> command = 0x33 -> 8-bit mode
		 * sleep(0.1) minimum
		 * send(0x20, RW = RS = 0) -> command = 0x2X -> 4-bit mode active
		 * now write upper 4 bits and lower 4 bits afterwards
		 */
		if(timebase_ms < 50) {
			// init sequence
			switch(timebase_ms) {
				case 19:
					lcd_send_init(0x30, LCD_REG_CONTROL);
					break;
				case 24:
					lcd_send_init(0x30, LCD_REG_CONTROL);
					break;
				case 25:
					lcd_send_init(0x30, LCD_REG_CONTROL);
					break;
#if LCD_USE_4BIT_MODE
				case 26:
					lcd_send_init(0x20, LCD_REG_CONTROL); // switch to 4-bit mode
					break;
#endif
				case 27:
#if LCD_USE_4BIT_MODE
					lcd_send(0x28, LCD_REG_CONTROL); // function set: 4-bit mode, 2 lines, 5x7 font
#else
					lcd_send(0x38, LCD_REG_CONTROL); // function set: 8-bit mode, 2 lines, 5x7 font
#endif
					break;
				case 28:
					lcd_send(0x08, LCD_REG_CONTROL); // display off
					break;
				case 30:
					lcd_send(0x01, LCD_REG_CONTROL); // display clear
					break;
				case 32:
					lcd_send(0x06, LCD_REG_CONTROL); // entry mode set: increment, no shift
					break;
				case 34:
					lcd_send(0x0C, LCD_REG_CONTROL); // display on, cursor off
					break;
				case 31:
					lcd_send(0x02, LCD_REG_CONTROL); // cursor home, needs 1.6 ms to execute
					break;
				case 49:
					lcd_send_string("L:");
					lcd_set_cursor_pos(1, 0);
					lcd_send_string("Hello World!");
					break;
			}
		} else {
			lcd_process();

			if((timebase_ms % 500) == 0) {
				fxp_t scaled_val;

				lcd_set_cursor_pos(1, 0);

				scaled_val = fxp_mult(fxp_from_int(adc_values[0]), VIN_SCALE);
				fxp_format(scaled_val, number, 1);
				fxp_right_align(number, msg, 4, ' ');
				lcd_send_string("I:");
				lcd_send_string(msg);
				lcd_send_string("V ");

				scaled_val = fxp_mult(fxp_from_int(adc_values[1]), VOUT_SCALE);
				fxp_format(scaled_val, number, 1);
				fxp_right_align(number, msg, 4, ' ');
				lcd_send_string("O:");
				lcd_send_string(msg);
				lcd_send_string("V ");

				lcd_set_cursor_pos(0, 8);

				scaled_val = fxp_mult(fxp_from_int(adc_values[2]), CURRENT_SCALE);
				scaled_val = fxp_mult(scaled_val, fxp_from_int(1000)); // A -> mA
				fxp_format(scaled_val, number, 0);
				fxp_right_align(number, msg, 4, ' ');
				lcd_send_string(msg);
				lcd_send_string("mA");
			}

			if((timebase_ms % 1000) == 10) {
				cpuload /= 1000;

				lcd_set_cursor_pos(0, 2);

				fxp_format_int((int32_t)cpuload, number);
				fxp_right_align(number, msg, 3, '0');
				lcd_send_string("0.");
				lcd_send_string(msg);
			}
		}

		// cpu load = timer1 value after main loop operations
		cpuload += timer_get_counter(TIM3);

		timebase_ms++;

		while(wait_frame);
		wait_frame = 1;
	}

	return 0;
}

/* Called when systick fires */
void sys_tick_handler(void)
{
	wait_frame = 0;
}

void tim3_isr(void)
{
	// check for update interrupt
	if(timer_interrupt_source(TIM3, TIM_SR_UIF)) {
		wait_frame = 0;
		timer_clear_flag(TIM3, TIM_SR_UIF);
	}
}

#if 0
void tim4_isr(void)
{
	uint32_t adcval, pwm_value;

	// check for update interrupt
	if(timer_interrupt_source(TIM4, TIM_SR_UIF)) {
		// read ADC value
		adcval = adc_read_regular(ADC2);

		pwm_value = AUDIO_PWM_PERIOD * adcval / 4096;
		timer_set_oc_value(TIM4, TIM_OC1, pwm_value);
		timer_set_oc_value(TIM4, TIM_OC2, pwm_value);

		dbg_audio_pwm_value = pwm_value;

		// start conversion for next cycle
		adc_start_conversion_direct(ADC2);

		timer_clear_flag(TIM4, TIM_SR_UIF);
	}
}
#endif

void hard_fault_handler(void)
{
	while (1);
}

/*
void usage_fault_handler(void)
{
	while (1);
}
*/