#define SYSTEM_CORE_CLOCK 48000000
#define APB_CLOCK SYSTEM_CORE_CLOCK
#include "ch32fun.h"
#include <stdio.h>
#include <stdlib.h>

#include "ch32v003_GPIO_branchless.h"
#include "systick.h"

// Hardware options
// #define RIGHT_CHANNEL_UNIT
#ifndef RIGHT_CHANNEL_UNIT
// Left channel unit
#undef REVERSED_PINS
#else
// Right channel unit
#define REVERSED_PINS
#endif

#define PWM_DRIVER

#include "display.h"
#include "pot_watch.h"
#include "vu.h"

enum display_mode {
	MODE_VU = 0,
	MODE_FREQ,
	MODE_AMP,
};
enum display_mode current_mode = MODE_VU;

#define KNOB_MODE_TIMEOUT 2000 // 2 seconds

#define PWM_DRIVER
#define KNOB_CHECK_INTERVAL 50 // 50ms/20Hz
#define VU_CHECK_INTERVAL 25  // 10ms/100Hz

void delay_ms(uint32_t ms) {
	// Delay for ms milliseconds
	uint32_t start = millis();
	while (millis() - start < ms) {
		// Do nothing
	}
}
void delay_us(uint32_t us) {
	// Delay for us microseconds
	uint32_t start = micros();
	while (micros() - start < us) {
		// Do nothing
	}
}

void larson_test_and_calibrate() {
	uint8_t i = 0;
	uint8_t dir = 1;
	// while (1) {
	for (int j = 0; j < 23; j++) {
		// Calibrate silence offsets
		calibrate_silence_offset();
		//  Set all pins to low
		for (int j = 0; j < 12; j++) {
			funDigitalWrite(NE_PINS[j], FUN_LOW);
		}
		calibrate_silence_offset();
		funDigitalWrite(NE_PINS[i], FUN_HIGH);
		calibrate_silence_offset();
		// Set current pin to high
		if (dir == 1) {
			i++;
			if (i == 11) {
				dir = -1;
			}
		} else {
			i--;
			if (i == 0) {
				dir = 1;
			}
		}
		calibrate_silence_offset();
		// funDigitalWrite(NE_PINS[i], FUN_HIGH);
		delay_ms(1000 / 12);
		calibrate_silence_offset();
	}

	// remove normal offset from calibration
	cal_off_avg -= 512;
	cal_off_peak -= 512;
}

int main() {
	SystemInit();
	printf("NeonVUEQ V0.1\n");
	printf("LPWorks 2025\n");
	// Initialize systick
	printf("initializing systick...\n");
	// ! Note, this breaks normal delay function, use delay_ms instead
	systick_init();

	printf("initializing GPIO...\n");
	// Enable IO ports
	GPIO_portEnable(GPIO_port_A);
	GPIO_portEnable(GPIO_port_C);
	GPIO_portEnable(GPIO_port_D);
	// Set pin modes
	// -- Analog inputs, PD2 VU_IN, PD3 PEAK_IN, PD6 F_POT, PD5 AMP_POT
	GPIO_pinMode(GPIO_port_D, 2, GPIO_pinMode_I_analog, GPIO_SPEED_IN);
	GPIO_pinMode(GPIO_port_D, 3, GPIO_pinMode_I_analog, GPIO_SPEED_IN);
	GPIO_pinMode(GPIO_port_D, 5, GPIO_pinMode_I_analog, GPIO_SPEED_IN);
	GPIO_pinMode(GPIO_port_D, 6, GPIO_pinMode_I_analog, GPIO_SPEED_IN);
	// -- Display outputs, push-pull
	for (int i = 0; i < 12; i++) {
		funPinMode(NE_PINS[i], GPIO_CFGLR_OUT_10Mhz_PP);
	}
	// -- Pot EN
	funPinMode(POT_EN, GPIO_CFGLR_OUT_10Mhz_PP);
	funDigitalWrite(POT_EN, FUN_HIGH); // Enable pot

	// Initialize ADC
	printf("initializing ADC...\n");
	GPIO_ADCinit();

	printf("Calibrating silence offset...\n");
	larson_test_and_calibrate();
	printf("Offset avg: %d, peak: %d\n", cal_off_avg, cal_off_peak);

	printf("Init done.\n");

	uint32_t last_VU_update_time = 0;
	uint32_t last_KNOB_update_time = 0;
	uint32_t pot_timeout = 0;

	while (1) {

		
		// Check display mode
		if (current_mode > MODE_VU && KNOB_CHECK_INTERVAL > 0) {
			// In freq/amp display mode
			// printf("Current mode: %d\n", current_mode);
			// Check if it is time to update the display
			if (millis() - last_KNOB_update_time >= KNOB_CHECK_INTERVAL) {
				// 0. Update timer
				last_KNOB_update_time = millis();

				// 1. Check if we have timed out, if so switch back to VU mode
				if (millis() > pot_timeout) {
					current_mode = MODE_VU;
					printf("Timeout: Switching to VU mode\n");
					pot_timeout = 0; // Reset timeout
					continue;		 // Skip to next loop
				}

				// 2. Read knobs
				uint16_t f_pot = GPIO_analogRead(GPIO_Ain6_D6);	  // F_POT
				uint16_t amp_pot = GPIO_analogRead(GPIO_Ain5_D5); // AMP_POT
				// 3. Switch to changed knob mode, but not VU_MODE
				uint8_t new_mode = pot_watch_check(&f_pot, &amp_pot);
				if (current_mode == MODE_FREQ && new_mode == MODE_AMP) {
					current_mode = MODE_AMP;
					printf("Switching to AMP mode\n");
				} else if (current_mode == MODE_AMP && new_mode == MODE_FREQ) {
					current_mode = MODE_FREQ;
					printf("Switching to FREQ mode\n");
				}
				// 4. Reset timeout if knobs were moved
				if (new_mode != 0) {
					pot_timeout = millis() + KNOB_MODE_TIMEOUT;
				}

				// printf("MODE: %s, F_POT: %d, AMP_POT: %d\n", (current_mode == MODE_FREQ) ? "MODE_FREQ" : "MODE_AMP", f_pot, amp_pot);

				// 5. Switch over knob display mode, and call display function
				switch (current_mode) {
				case MODE_FREQ:
					// Display frequency knob
					display_KNOB_freq(&f_pot);
					break;
				case MODE_AMP:
					// Display amplitude
					display_KNOB_amp(&amp_pot);
					break;
				default:
					// Shouldn't happen
					continue; // Skip to next loop
				}
			}

		} else {
			// In VU mode
			// Check if it is time to update the display
			if (millis() - last_VU_update_time >= VU_CHECK_INTERVAL) { // 100Hz refresh rate, max 800Hz ish (1250uS)

				// 0. Update timer
				last_VU_update_time = millis();
				// uint32_t timer_start = micros();

				// 1. Read ADCs, takes ~1.2ms
				uint16_t vu = GPIO_analogRead(GPIO_Ain3_D2);   // VU_IN
				uint16_t peak = GPIO_analogRead(GPIO_Ain4_D3); // PEAK_IN
				
				uint16_t f_pot = GPIO_analogRead(GPIO_Ain6_D6);	  // F_POT
				uint16_t amp_pot = GPIO_analogRead(GPIO_Ain5_D5); // AMP_POT

				// printf("VU_MODE: VU: %d, PEAK: %d, F_POT: %d, AMP_POT: %d\n", vu, peak, f_pot, amp_pot);

				// 2. Check if knobs were moved
				
				current_mode = pot_watch_check(&f_pot, &amp_pot);
				// printf("Current mode: %d\n", current_mode);
				if (current_mode != MODE_VU && millis() > KNOB_MODE_TIMEOUT*2) {
					// Knob was moved, set inital timeout
					pot_timeout = millis() + KNOB_MODE_TIMEOUT;
					continue; // Skip VU update
				}
				

				// 3. Update VU display
				display_VU(&vu, &peak);
				// printf("VU Update took %d us out of %d us budget.\n", (micros() - timer_start),
				//	   VU_CHECK_INTERVAL * 1000);
			}
		}


		// Yield for software PWM
		#ifdef PWM_DRIVER
			display_pwm_yield();
		#endif
		
		
	}
}
