#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include "lcd/HD44780.h"
#include "sound_analysis.h"
#include "adc/adc.h"
#include "guitar_tuner.h"

// LEDs/buttons
#define LED1 PB1
#define LED2 PB2
#define LED3 PB7
#define LED4 PE6
#define BTN1 PB3
#define BTN2 PC6

void turn_led2_on(void)
{
    PORTB |= (1 << LED2);
}

void turn_led2_off(void)
{
    PORTB &= ~(1 << LED2);
}

void init_guitar_tuner(void)
{
    // --- Clock ---
    CLKSEL0 = 0b00010101;
    CLKSEL1 = 0b00001111;
    CLKPR = 0x80;
    CLKPR = 0;

    MCUCR |= (1 << JTD);
    MCUCR |= (1 << JTD); // Disable JTAG twice

    // --- LEDs ---
    DDRB |= (1 << LED1) | (1 << LED2) | (1 << LED3);
    DDRE |= (1 << LED4);

    // --- Buttons ---
    DDRB &= ~(1 << BTN1);
    PORTB |= (1 << BTN1);
    DDRC &= ~(1 << BTN2);
    PORTC |= (1 << BTN2);

    // --- Init ---
    ADC_Init();

    HD44780_Initialize();
    HD44780_WriteCommand(LCD_ON | CURSOR_NONE);
    HD44780_WriteCommand(LCD_CLEAR);
    HD44780_WriteCommand(LCD_HOME);
    HD44780_WriteCommand(LCD_INCR_RIGHT);
    _delay_ms(50); // prevents freezes

    display_message("Initiazing screen");
    _delay_ms(500);
    display_message("Not capturing");
}

float log2f_approx(float x)
{
    if (x <= 0.0f)
        return 0.0f;

    int exponent = 0;
    float mantissa = x;

    while (mantissa >= 2.0f)
    {
        mantissa *= 0.5f;
        exponent++;
    }
    while (mantissa < 1.0f)
    {
        mantissa *= 2.0f;
        exponent--;
    }

    float f = mantissa - 1.0f;
    float log2_m = f - (f * f) / 2.0f + (f * f * f) / 3.0f;

    return (float)exponent + log2_m;
}

void frequency_to_note(float freq, char *note_name)
{
    if (freq <= 0.0f)
    {
        sprintf(note_name, "---");
        return;
    }

    const char *note_names[] = {
        "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B"};

    float semitones = 12.0f * log2f_approx(freq / 440.0f);
    int midi = (int)(69 + semitones + 0.5f);

    int note = midi % 12;
    int octave = (midi / 12) - 1;

    sprintf(note_name, "%s%d", note_names[note], octave);
}

void loop_guitar_tuner(char *note_buf)
{
    // --- Blink LED1 to show program is alive ---
    PORTB ^= (1 << LED1);
    _delay_ms(200);

    // --- Wait for BTN2 press ---
    static uint8_t btn2_was_pressed = 0;

    if (!(PINC & (1 << BTN2)))
    { // Button pressed (active low)
        if (!btn2_was_pressed)
        {
            btn2_was_pressed = 1;

            PORTE |= (1 << LED4);
            display_message("Capturing...");

            collect_audio_samples();

            int16_t peak_volume = get_max_sample();
            float freq = detect_frequency();

            frequency_to_note(freq, note_buf);

            PORTE &= ~(1 << LED4);

            char result[24];
            snprintf(result, sizeof(result),
                     "V:%3d F:%4uHz N:%s",
                     peak_volume, (uint16_t)freq, note_buf);

            display_message(result);
            _delay_ms(DISPLAY_HOLD_MS);

            display_message("Not capturing");
        }
    }
    else
    {
        btn2_was_pressed = 0; // Button released
    }
}