#ifndef DISPLAY_H
#define DISPLAY_H

#include "ch32fun.h"
#include <stdint.h>
#include <stdio.h>

#include "rev_log_to_lin.h"

#include "pot_watch.h"

// Pin Definitions
#define NE_0 PD7
#define NE_1 PA1
#define NE_2 PA2
#define NE_3 PD0
#define NE_4 PC0
#define NE_5 PC1
#define NE_6 PC2
#define NE_7 PC3
#define NE_8 PC4
#define NE_9 PC5
#define NE_10 PC6
#define NE_11 PC7

#ifdef REVERSED_PINS
	const uint8_t NE_PINS[12] = {NE_11, NE_10, NE_9, NE_8, NE_7, NE_6, NE_5, NE_4, NE_3, NE_2, NE_1, NE_0};
#else
	const uint8_t NE_PINS[12] = {NE_0, NE_1, NE_2, NE_3, NE_4, NE_5, NE_6, NE_7, NE_8, NE_9, NE_10, NE_11};
#endif

#ifdef PWM_DRIVER
	uint8_t pwm_chans[12] = {0}; // PWM levels for each pin
	uint8_t peak_index = 0; // Index for peak indicator
#endif


uint16_t cal_off_avg = 0; // Calibration offset for average VU
uint16_t cal_off_peak = 0; // Calibration offset for peak VU


// Bitmap driver function
static inline void display_bitmap(uint16_t level) {
	// Draw bitmap to screen
	level = level & 0x0FFF; // Mask to 12 bits
							// LSB => NE_0, MSB => NE_11
	for (int i = 0; i < 12; i++) {
		if (level & (1 << i)) {
			funDigitalWrite(NE_PINS[i], FUN_HIGH);
		} else {
			funDigitalWrite(NE_PINS[i], FUN_LOW);
		}
	}
}

// PWM Driver function
#if defined(PWM_DRIVER) || defined(PWM_DRIVER_KNOB)
static uint8_t pwm_counter = 0;

__attribute__((always_inline)) static inline void display_pwm_yield() {
	// static uint8_t pwm_counter = 0; // Static counter to track PWM cycle
	// // Output PWM levels based on counter
	// for (uint8_t i = 0; i < 12; i++) {
	// 	funDigitalWrite(NE_PINS[i], pwm_counter < pwm_levels[i]);
	// }
	// // Increment counter and reset
	// pwm_counter+=64;
	// if (pwm_counter >= 255) {
	// 	pwm_counter = 0; // Reset counter after reaching max value
	// }
	
	// advance by 64; wraps 192→0 automatically on overflow
	pwm_counter += 32;

	// copy into locals
	uint8_t cnt = pwm_counter;
	uint8_t const *lvl = pwm_chans;
	uint8_t const *pin = NE_PINS;

	uint8_t const pi = peak_index;
	uint8_t peak_led   = (pi > 0 && pi <= 12) ? (pi - 1) : 0xFF;
	uint8_t idx = 0;

	// process 12 channels in 3 unrolled steps of 4
	for (uint8_t i = 0; i < 12; i +=4) {
		// funDigitalWrite(pin[0], cnt < lvl[0]);
		// funDigitalWrite(pin[1], cnt < lvl[1]);
		// funDigitalWrite(pin[2], cnt < lvl[2]);
		// funDigitalWrite(pin[3], cnt < lvl[3]);
		funDigitalWrite(pin[0], (idx == peak_led) | (cnt < lvl[0]));  idx++;
		funDigitalWrite(pin[1], (idx == peak_led) | (cnt < lvl[1]));  idx++;
		funDigitalWrite(pin[2], (idx == peak_led) | (cnt < lvl[2]));  idx++;
		funDigitalWrite(pin[3], (idx == peak_led) | (cnt < lvl[3]));  idx++;
		pin += 4;
		lvl += 4;
	}

}

// dB Mapping Functions
static const uint16_t VU_THRESHOLDS[14] = {
    517,   // start of segment 0
    519, // start of segment 1
    522, // 2
    526, // 3
    532, // 4
    541, // 5
    552, // 6
    569, // 7
    592, // 8
    625, // 9
    672, // 10
    738, // 11
    832, // 12
    1024 // end sentinel
};

#endif

uint8_t map_adc2VUind(uint16_t *adc) {
	*adc &= 0x3FF; // Mask to 10 bits
	//*adc -= 511; // Remove VGND offset

	// dumb mapping for now, use binary search for better performance
	if (*adc >= 832)
		return 12; // +3
	if (*adc >= 738)
		return 11; // 0
	if (*adc >= 672)
		return 10; // -3
	if (*adc >= 625)
		return 9; // -6
	if (*adc >= 592)
		return 8; // -9
	if (*adc >= 569)
		return 7; // -12
	if (*adc >= 552)
		return 6; // -15
	if (*adc >= 541)
		return 5; // -18
	if (*adc >= 532)
		return 4; // -21
	if (*adc >= 526)
		return 3; // -24
	if (*adc >= 522)
		return 2; // -27
	if (*adc >= 519)
		return 1; // -30
	return 0;	  // <= -33
}

uint8_t map_adc2VUPeakind(uint16_t *adc) {
	*adc &= 0x3FF; // Mask to 10 bits
	//*adc -= 511; // Remove VGND offset

	// dumb mapping for now, use binary search for better performance
	if (*adc >= 1015)
		return 12; // +3
	if (*adc >= 868)
		return 11; // 0
	if (*adc >= 764)
		return 10; // -3
	if (*adc >= 690)
		return 9; // -6
	if (*adc >= 638)
		return 8; // -9
	if (*adc >= 601)
		return 7; // -12
	if (*adc >= 575)
		return 6; // -15
	if (*adc >= 557)
		return 5; // -18
	if (*adc >= 544)
		return 4; // -21
	if (*adc >= 534)
		return 3; // -24
	if (*adc >= 528)
		return 2; // -27
	if (*adc >= 523)
		return 1; // -30
	return 0;	  // <= -33
}

#ifdef DEBUG
// Debug Serial display functions
void display_serial(uint16_t level) {
	// Draw bitmap to serial
	level = level & 0x0FFF; // Mask to 12 bits
	// LSB => NE_0, MSB => NE_11
	printf("Display: ");
	for (int i = 0; i < 12; i++) {
		printf("[");
		if (level & (1 << i)) {
			printf("X");
		} else {
			printf(" ");
		}
		printf("]");
	}
	printf("\n");
}

void display_KNOB_lin_debug(uint16_t *amp_pot) {
	*amp_pot &= 0x03FF; // Mask to 10 bits
	// Linear mapping to 12 bit bitmap, with all indicators below on
	uint8_t level = (*amp_pot * 12) / 1023;
	uint16_t bitmap = (level == 0) ? 0x0000 : (1 << level) - 1;
	// Display
	// display_serial(bitmap);
	display_bitmap(bitmap);
}

void display_VU_lin_debug(uint16_t *avg, uint16_t *peak) {
	*avg &= 0x03FF;	 // Mask to 10 bits
	*peak &= 0x03FF; // Mask to 10 bits
	// Linear mapping to 12 bit bitmap, with all indicators below on
	//*avg = *avg - 511;
	//*peak = *peak - 511;
	*avg -= 12;
	*peak -= 12; // offset wrong for VGND?
	// uint8_t level = (*avg * 12) / 512;
	uint8_t level = map_adc2VUind(avg);
	uint16_t bitmap = (level == 0) ? 0x0000 : (1 << level) - 1;
	// Extract single peak bit
	// uint8_t peak_level = (*peak * 12) / 512;
	uint8_t peak_level = map_adc2VUPeakind(peak);

	// printf("VU: %d avg, %d peak, %d level, %d peak_level\n", *avg, *peak, level, peak_level);

	if (peak_level > level && 1) {
		bitmap |= (1 << (peak_level - 1));
	}
	// Display
	// display_serial(bitmap);
	display_bitmap(bitmap);
}
#endif

// Mapping functions
// Take 10-bit ADC readings --> Outputs 12-bit bitmap or PWM levels
#ifndef PWM_DRIVER
uint16_t display_map_VU(uint16_t *avg, uint16_t *peak) {
	*avg &= 0x03FF;	 // Mask to 10 bits
	*peak &= 0x03FF; // Mask to 10 bits

	uint8_t level = map_adc2VUind(avg);
	uint16_t bitmap = (level == 0) ? 0x0000 : (1 << level) - 1;
	// Extract single peak bit
	uint8_t peak_level = map_adc2VUPeakind(peak);

	// printf("VU: %d avg, %d peak, %d level, %d peak_level\n", *avg, *peak, level, peak_level);

	if (peak_level > level && 1) {
		bitmap |= (1 << (peak_level - 1));
	}
	return bitmap;
}
#else
void display_map_VU_pwm(uint16_t *avg, uint16_t *peak) {
	*avg &= 0x03FF;	 // Mask to 10 bits
	*peak &= 0x03FF; // Mask to 10 bits

	int8_t segment = -1;
	for (uint8_t i = 0; i < 12; i++) {
		if (*avg < VU_THRESHOLDS[i+1]) {
			segment = i;
			break;
		}
	}

	// Clear all PWM levels to 0
	for (uint8_t i = 0; i < 12; i++) {
		pwm_chans[i] = 0;
	}

	// Calculate fade factor (0 to 255) for smooth transition
	if (segment > 0) {
		uint16_t seg_start = VU_THRESHOLDS[segment];
		uint16_t seg_end = VU_THRESHOLDS[segment + 1];
		uint16_t seg_len = seg_end - seg_start;
		uint8_t fade = (seg_len > 1) ? (uint8_t)(((uint32_t)(*avg - seg_start) * 255) / (seg_len - 1)) : 255;
		// fade current segment
		pwm_chans[segment] = fade;
		// Fully on below segment
		for (uint8_t i = 0; i < segment; i++) {
			pwm_chans[i] = 255; // Full brightness for all segments below
		}
		// Overlay peak indicator bit
		uint16_t peak_bit = map_adc2VUPeakind(peak);
		uint8_t idx = __builtin_ctz(peak_bit);
		if (idx < 12) {
			pwm_chans[idx] = 255; // Set peak bit to full brightness
			peak_index = idx; // Update peak index
		}
	}
	
}
#endif

#ifndef PWM_DRIVER
uint16_t display_map_KNOB_amp(uint16_t *amp_pot) {
	*amp_pot &= 0x03FF; // Mask to 10 bits
	// Linear mapping to 24 half-levels (double 12), each ~42.625 units
    uint16_t level = (*amp_pot * 12) / 1023;
	uint16_t half_level = (*amp_pot *24) / 1023;
	uint8_t is_half = half_level % 2; // 0 for full level, 1 for in-between
	uint16_t bitmap = 0;
    // Fill from center out
	if (level < 6) {
		for (uint8_t i = 6; i > level; i--) {
			bitmap |= (1 << (i-1)); // Set bits from level 0-6
		}
		// bitmap = bitmap >> 1;
	} else if (level >= 6) {
		for (uint8_t i = 6; i <= level; i++) {
			bitmap |= (1 << i); // Set bits from level 7-11
		}
		// bitmap = bitmap << 1;
	}
	// Light both leds if in between
	if (is_half && level == 5) {
		bitmap = 0b0000000001100000;
	}
	
    return bitmap;
}
#else
void display_map_KNOB_amp_pwm(uint16_t *amp_pot) {
	*amp_pot &= 0x03FF; // Mask to 10 bits
	// Calculate position
	// Calculate segment size for 12 lamps (1024 / 12 ≈ 85.33)
    uint16_t segment_size = 1024 / 12;
	// Find which segment the pot_value falls into (0 to 11)
    uint8_t segment = *amp_pot / segment_size;
    // Calculate position within the segment (0 to segment_size-1)
    uint16_t position_in_segment = *amp_pot % segment_size;

	// If pot_value is exactly at the max (1023), cap segment to 11
    if (segment >= 12) {
        segment = 11;
        position_in_segment = segment_size - 1;
    }

	// Calculate fade factor (0 to 255) for smooth transition
    uint8_t fade = (uint8_t)((position_in_segment * 255) / segment_size);

	// Clear all PWM levels to 0
    for (uint8_t i = 0; i < 12; i++) {
        pwm_chans[i] = 0;
    }

	// Set the PWM level for the corresponding segments
	// If not at the start of a segment, fade between two lamps
    if (position_in_segment > 0 && segment < 11 && !(segment == 0 && position_in_segment < 16)) {
        // Current lamp fades out, next lamp fades in
        pwm_chans[segment] = 255 - fade;
        pwm_chans[segment + 1] = fade;
    } else {
        // Light only the current lamp at full brightness
        pwm_chans[segment] = 255;
    }

	// Fill from center out
	if (segment <= 5) {
		for (uint8_t i = 5; i > segment; i--) {
			pwm_chans[i] = 255; // Set bits from level 0-6
		}
	} else if (segment >=6) {
		for (uint8_t i = 6; i <= segment; i++) {
			pwm_chans[i] = 255; // Set bits from level 7-11
		}
	}
}
#endif

#ifndef PWM_DRIVER
uint16_t display_map_KNOB_freq(uint16_t *f_pot) {
	*f_pot &= 0x03FF; // Mask to 10 bits
	*f_pot = map_log_to_linear(*f_pot);
	// Linear mapping to 24 half-levels (double 12), each ~42.625 units
    uint16_t half_level = (*f_pot * 24) / 1023;
    uint8_t level = half_level / 2; // Integer level (0-11)
    uint8_t is_half = half_level % 2; // 0 for full level, 1 for in-between
    uint16_t bitmap = 1 << level; // Set current level bit
    if (is_half && level < 11) {
        bitmap |= 1 << (level + 1); // Set next level bit if in-between
    }
    return bitmap;
}
#else
void display_map_KNOB_freq_pwm(uint16_t *f_pot) {
	*f_pot &= 0x03FF; // Mask to 10 bits
	*f_pot = map_log_to_linear(*f_pot);
	// Calculate position
	// Calculate segment size for 12 lamps (1024 / 12 ≈ 85.33)
    uint16_t segment_size = 1024 / 12;
	// Find which segment the pot_value falls into (0 to 11)
    uint8_t segment = *f_pot / segment_size;
    // Calculate position within the segment (0 to segment_size-1)
    uint16_t position_in_segment = *f_pot % segment_size;

	// If pot_value is exactly at the max (1023), cap segment to 11
    if (segment >= 12) {
        segment = 11;
        position_in_segment = segment_size - 1;
    }

	// Calculate fade factor (0 to 255) for smooth transition
    uint8_t fade = (uint8_t)((position_in_segment * 255) / segment_size);

	// Clear all PWM levels to 0
    for (uint8_t i = 0; i < 12; i++) {
        pwm_chans[i] = 0;
    }

	// Set the PWM level for the corresponding segments
	// If not at the start of a segment, fade between two lamps
    if (position_in_segment > 0 && segment < 11 && !(segment == 0 && position_in_segment < 16)) {
        // Current lamp fades out, next lamp fades in
        pwm_chans[segment] = 255 - fade;
        pwm_chans[segment + 1] = fade;
    } else {
        // Light only the current lamp at full brightness
        pwm_chans[segment] = 255;
    }
}
#endif

// Knob display functions
void display_KNOB_amp(uint16_t *amp_pot) {
	#ifdef PWM_DRIVER
	// Map to PWM levels
	display_map_KNOB_amp_pwm(amp_pot);
#else
	// Map to bitmap
	uint16_t bitmap = display_map_KNOB_amp(amp_pot);
	// Display
	display_bitmap(bitmap);
#endif
}

void display_KNOB_freq(uint16_t *freq_pot) {
	#ifdef PWM_DRIVER
	// Map to PWM levels
	display_map_KNOB_freq_pwm(freq_pot);
#else
	// Map to bitmap
	uint16_t bitmap = display_map_KNOB_freq(freq_pot);
	// Display
	display_bitmap(bitmap);
#endif
}

// VU display functions
void display_VU(uint16_t *avg, uint16_t *peak) {
	// Apply calibration offsets
	*avg = *avg - cal_off_avg;
	*peak = *peak - cal_off_peak;

#ifdef PWM_DRIVER
	// Map to PWM levels
	display_map_VU_pwm(avg, peak);
#else
	// Map to bitmap
	uint16_t bitmap = display_map_VU(avg, peak);
	// Display
	display_bitmap(bitmap);
#endif
}




// calibration function

void calibrate_silence_offset() {
	// Read the ADC values for VU_IN and PEAK_IN
	uint16_t vu = GPIO_analogRead(GPIO_Ain3_D2);   // VU_IN
	uint16_t peak = GPIO_analogRead(GPIO_Ain4_D3); // PEAK_IN
	// Keep running average
	cal_off_avg = (cal_off_avg + vu) / 2;
	cal_off_peak = (cal_off_peak + peak) / 2;
}

#endif
