/*
*     Example Code Three - Dual Channel Sequencer (COMPLETE)
*     - Alle TODOs implementiert
*     - VCO Gates, Recording LED, Metronome
*/
#include "FIRMWARE_DEF.h"
#include "FIRMWARE.h"

byte pins_keyboard_row[N_KEYBOARD_ROW] = {PIN_K_R0, PIN_K_R1, PIN_K_R2, PIN_K_R3};
byte pins_keyboard_col[N_KEYBOARD_COL] = {PIN_K_C0, PIN_K_C1, PIN_K_C2};

Keyboard keyboard(N_KEYBOARD_ROW, N_KEYBOARD_COL, pins_keyboard_row, pins_keyboard_col);

Adafruit_MCP4728 MCP4728;
MCP4728_channel_t cvMap[N_CV_GATES] = {MCP4728_CHANNEL_A, MCP4728_CHANNEL_B};
uint16_t keyToVoltage[N_KEYBOARD_ROW*N_KEYBOARD_COL] = {
    1*83, 5*83, 9*83,
    2*83, 6*83, 10*83,
    3*83, 7*83, 11*83,
    4*83, 8*83, 12*83
};

CV cv(&MCP4728, &Wire, N_CV_GATES, cvMap, keyToVoltage, N_KEYBOARD_ROW, N_KEYBOARD_COL);

SequencerBlock sb1(30000, N_MAX_SEQ_STEPS);
SequencerBlock sb2(30000, N_MAX_SEQ_STEPS);

// Button States
struct ButtonState {
    bool current;
    bool last;
    unsigned long lastDebounceTime;
};

ButtonState btn_sb1_rec;
ButtonState btn_sb1_play;
ButtonState btn_sb2_rec;
ButtonState btn_sb2_play;
ButtonState btn_metronome;

const unsigned long DEBOUNCE_DELAY = 50;

static bool seq1_loop_active = false;
static bool seq2_loop_active = false;
static uint16_t last_voltage_ch1 = 0xFFFF;
static uint16_t last_voltage_ch2 = 0xFFFF;

bool readButton(byte pin, ButtonState &state)
{
    bool reading = digitalRead(pin) == LOW;
    bool buttonPressed = false;
    
    if(reading != state.last)
    {
        state.lastDebounceTime = millis();
    }
    
    if((millis() - state.lastDebounceTime) > DEBOUNCE_DELAY)
    {
        if(reading != state.current)
        {
            state.current = reading;
            if(state.current == true)
            {
                buttonPressed = true;
            }
        }
    }
    
    state.last = reading;
    return buttonPressed;
}

void initButtons()
{
    pinMode(PIN_SB_1_REC, INPUT_PULLUP);
    pinMode(PIN_SB_1_PLAY, INPUT_PULLUP);
    pinMode(PIN_SB_2_REC, INPUT_PULLUP);
    pinMode(PIN_SB_2_PLAY, INPUT_PULLUP);
    pinMode(PIN_B_METRONOME, INPUT_PULLUP);
    
    btn_sb1_rec.current = false;
    btn_sb1_rec.last = false;
    btn_sb1_rec.lastDebounceTime = 0;
    
    btn_sb1_play.current = false;
    btn_sb1_play.last = false;
    btn_sb1_play.lastDebounceTime = 0;
    
    btn_sb2_rec.current = false;
    btn_sb2_rec.last = false;
    btn_sb2_rec.lastDebounceTime = 0;
    
    btn_sb2_play.current = false;
    btn_sb2_play.last = false;
    btn_sb2_play.lastDebounceTime = 0;
    
    btn_metronome.current = false;
    btn_metronome.last = false;
    btn_metronome.lastDebounceTime = 0;
}

void initOutputs()
{
    // VCO Gates
    pinMode(PIN_VCO1_EN, OUTPUT);
    pinMode(PIN_VCO2_EN, OUTPUT);
    digitalWrite(PIN_VCO1_EN, LOW);
    digitalWrite(PIN_VCO2_EN, LOW);
    
    // Recording LED (active-low)
    pinMode(PIN_REC, OUTPUT);
    digitalWrite(PIN_REC, HIGH); // OFF
    
    // Metronome LED (active-low)
    pinMode(PIN_L_METRONOME, OUTPUT);
    digitalWrite(PIN_L_METRONOME, HIGH); // OFF
    
    // BPM Potentiometer
    pinMode(PIN_BPM, INPUT);
}

void handleSequencerButtons()
{
    // ===== Sequencer 1 Record Button =====
    if(readButton(PIN_SB_1_REC, btn_sb1_rec))
    {
        if(sb1.isRecording())
        {
            sb1.stopRecord();
            Serial.printf("\n\r[SEQ1] Recording stopped. Steps: %i, Duration: %ims", 
                         sb1.getStepCount(), sb1.getTotalDuration());
        }
        else
        {
            if(sb1.isPlaying()) sb1.stopPlay();
            sb1.startRecord();
            last_voltage_ch1 = 0xFFFF;
            last_voltage_ch2 = 0xFFFF;
            Serial.printf("\n\r[SEQ1] Recording started (2 channels)...");
        }
    }
    
    // ===== Sequencer 1 Play Button =====
    if(readButton(PIN_SB_1_PLAY, btn_sb1_play))
    {
        if(!sb1.isPlaying())
        {
            if(sb1.isRecording()) sb1.stopRecord();
            sb1.setLoop(false);
            seq1_loop_active = false;
            sb1.startPlay();
            Serial.printf("\n\r[SEQ1] Playback started (single)\n\r\tSteps: %i, Duration: %ims", 
                         sb1.getStepCount(), sb1.getTotalDuration());
        }
        else if(!seq1_loop_active)
        {
            sb1.setLoop(true);
            seq1_loop_active = true;
            Serial.printf("\n\r[SEQ1] Loop activated");
        }
        else
        {
            sb1.stopPlay();
            seq1_loop_active = false;
            Serial.printf("\n\r[SEQ1] Playback stopped");
        }
    }
    
    // ===== Sequencer 2 Record Button =====
    if(readButton(PIN_SB_2_REC, btn_sb2_rec))
    {
        if(sb2.isRecording())
        {
            sb2.stopRecord();
            Serial.printf("\n\r[SEQ2] Recording stopped. Steps: %i, Duration: %ims", 
                         sb2.getStepCount(), sb2.getTotalDuration());
        }
        else
        {
            if(sb2.isPlaying()) sb2.stopPlay();
            sb2.startRecord();
            last_voltage_ch1 = 0xFFFF;
            last_voltage_ch2 = 0xFFFF;
            Serial.printf("\n\r[SEQ2] Recording started (2 channels)...");
        }
    }
    
    // ===== Sequencer 2 Play Button =====
    if(readButton(PIN_SB_2_PLAY, btn_sb2_play))
    {
        if(!sb2.isPlaying())
        {
            if(sb2.isRecording()) sb2.stopRecord();
            sb2.setLoop(false);
            seq2_loop_active = false;
            sb2.startPlay();
            Serial.printf("\n\r[SEQ2] Playback started (single)\n\r\tSteps: %i, Duration: %ims", 
                         sb2.getStepCount(), sb2.getTotalDuration());
        }
        else if(!seq2_loop_active)
        {
            sb2.setLoop(true);
            seq2_loop_active = true;
            Serial.printf("\n\r[SEQ2] Loop activated");
        }
        else
        {
            sb2.stopPlay();
            seq2_loop_active = false;
            Serial.printf("\n\r[SEQ2] Playback stopped");
        }
    }
}

static bool metronome_enabled = false;
static uint16_t current_bpm = 120;
static unsigned long last_beat_time = 0;
static unsigned long last_pulse_end_time = 0;
static bool metronome_led_on = false;

void updateMetronome()
{
    unsigned long now = millis();
    
    // BPM von Potentiometer lesen (alle 100ms)
    static unsigned long last_bpm_read = 0;
    if((now - last_bpm_read) > 100)
    {
        int adc_value = analogRead(PIN_BPM);
        // Map ADC (0-4095) zu BPM (40-240)
        current_bpm = map(adc_value, 0, 4095, 40, 240);
        last_bpm_read = now;
    }
    
    // Metronome Button (Toggle)
    if(readButton(PIN_B_METRONOME, btn_metronome))
    {
        metronome_enabled = !metronome_enabled;
        Serial.printf("\n\r[METRONOME] %s (BPM: %d)", 
                     metronome_enabled ? "ON" : "OFF", current_bpm);
        
        if(!metronome_enabled)
        {
            digitalWrite(PIN_L_METRONOME, HIGH); // Active-low: HIGH = OFF
            metronome_led_on = false;
        }
    }
    
    if(!metronome_enabled) return;
    
    // Berechne Beat-Intervall in ms
    unsigned long beat_interval = 60000UL / current_bpm;
    
    // Neue Beat?
    if((now - last_beat_time) >= beat_interval)
    {
        digitalWrite(PIN_L_METRONOME, LOW); // Active-low: LOW = ON
        metronome_led_on = true;
        last_beat_time = now;
        last_pulse_end_time = now + 50; // 50ms Pulse
    }
    
    // Pulse beenden?
    if(metronome_led_on && (now >= last_pulse_end_time))
    {
        digitalWrite(PIN_L_METRONOME, HIGH); // Active-low: HIGH = OFF
        metronome_led_on = false;
    }
}

void updateVCOGates(bool cv1_active, bool cv2_active)
{
    // PIN_VCO1_EN: HIGH wenn CV1 aktiv (Key mapped to CV-Gate 1)
    digitalWrite(PIN_VCO1_EN, cv1_active ? HIGH : LOW);
    
    // PIN_VCO2_EN: HIGH wenn CV2 aktiv (Key mapped to CV-Gate 2)
    digitalWrite(PIN_VCO2_EN, cv2_active ? HIGH : LOW);
}

void updateRecordingLED()
{
    // PIN_REC: Active-low (LOW = LED ON)
    bool any_recording = sb1.isRecording() || sb2.isRecording();
    digitalWrite(PIN_REC, any_recording ? LOW : HIGH);
}


void setup()
{
    Serial.begin(BAUDRATE);
    delay(2000);
    Serial.printf("\n\r=== COMPLETE VERSION with TODOs ===");
    Serial.printf("\n\rSerial OK!");

    keyboard.begin();
    
    unsigned long timeout = millis() + 5000;
    while(!cv.begin(PIN_SDA, PIN_SCL))
    {
        Serial.printf("\n\r[ERROR] CV initialization failed. Retrying...");
        delay(500);
        
        if(millis() > timeout)
        {
            Serial.printf("\n\r[FATAL] CV initialization timeout! Check I2C connection.");
            break;
        }
    }
    
    Serial.printf("\n\r[OK] CV initialized");
    
    initButtons();
    initOutputs();
    
    sb1.setLoop(false);
    sb2.setLoop(false);
    
    Serial.printf("\n\r=== Dual-Channel Sequencer System Started ===");
    Serial.printf("\n\rFeatures:");
    Serial.printf("\n\r  - VCO1/VCO2 Gate Outputs");
    Serial.printf("\n\r  - Recording LED Indicator");
    Serial.printf("\n\r  - BPM Metronome (40-240 BPM)");
    Serial.printf("\n\r==============================================\n\r");
}

void loop() 
{
    // ===== DEBUG HEARTBEAT =====
    static unsigned long lastDebugPrint = 0;
    static unsigned long loopCounter = 0;
    
    loopCounter++;
    
    if(millis() - lastDebugPrint > 5000)
    {
        Serial.printf("\n\r[HEARTBEAT] Loop: %lu | BPM: %d | Metro: %s", 
                     loopCounter, current_bpm, metronome_enabled ? "ON" : "OFF");
        Serial.printf("\n\r[DEBUG] SB1: Rec=%d, Play=%d, Steps=%d", 
                     sb1.isRecording(), sb1.isPlaying(), sb1.getStepCount());
        Serial.printf("\n\r[DEBUG] SB2: Rec=%d, Play=%d, Steps=%d", 
                     sb2.isRecording(), sb2.isPlaying(), sb2.getStepCount());
        lastDebugPrint = millis();
    }
    
    // ===== NON-BLOCKING TIMING =====
    static unsigned long lastLoopTime = 0;
    unsigned long now = millis();
    const unsigned long LOOP_INTERVAL = 10;
    
    if((now - lastLoopTime) < LOOP_INTERVAL)
    {
        return;
    }
    lastLoopTime = now;
    
    // ===== UPDATE FUNCTIONS =====
    keyboard.update();
    handleSequencerButtons();
    updateMetronome();
    updateRecordingLED();
    
    sb1.update();
    sb2.update();
    
    int n = keyboard.getQueueLength();
    
    // Aktuelle Spannungen ermitteln
    uint16_t voltage_ch1 = 0;
    uint16_t voltage_ch2 = 0;
    bool cv1_active = false;
    bool cv2_active = false;
    
    if(n > 0)
    {
        Key k1 = keyboard.getQueue(0);
        if(!isNotKey(k1))
        {
            voltage_ch1 = keyToVoltage[k1.row * N_KEYBOARD_COL + k1.col];
            cv1_active = true;
        }
    }
    
    if(n > 1)
    {
        Key k2 = keyboard.getQueue(1);
        if(!isNotKey(k2))
        {
            voltage_ch2 = keyToVoltage[k2.row * N_KEYBOARD_COL + k2.col];
            cv2_active = true;
        }
    }
    
    // Recording
    bool voltageChanged = (voltage_ch1 != last_voltage_ch1) || (voltage_ch2 != last_voltage_ch2);
    
    if(sb1.isRecording() && voltageChanged)
    {
        sb1.addStep(voltage_ch1, voltage_ch2);
        last_voltage_ch1 = voltage_ch1;
        last_voltage_ch2 = voltage_ch2;
    }
    
    if(sb2.isRecording() && voltageChanged)
    {
        sb2.addStep(voltage_ch1, voltage_ch2);
        last_voltage_ch1 = voltage_ch1;
        last_voltage_ch2 = voltage_ch2;
    }
    
    // CV-Ausgabe & VCO Gates
    if(sb1.isPlaying())
    {
        uint16_t seq_v1 = sb1.getCurrentVoltageCh1();
        uint16_t seq_v2 = sb1.getCurrentVoltageCh2();
        cv.setVoltage(0, seq_v1);
        cv.setVoltage(1, seq_v2);
        
        // KORREKT: Nutze isCurrentStepActive() statt Spannung > 0
        // Da 0V eine gültige Note sein kann!
        bool gate_active = sb1.isCurrentStepActive();
        updateVCOGates(gate_active, gate_active);
    }
    else if(sb2.isPlaying())
    {
        uint16_t seq_v1 = sb2.getCurrentVoltageCh1();
        uint16_t seq_v2 = sb2.getCurrentVoltageCh2();
        cv.setVoltage(0, seq_v1);
        cv.setVoltage(1, seq_v2);
        
        bool gate_active = sb2.isCurrentStepActive();
        updateVCOGates(gate_active, gate_active);
    }
    else
    {
        // Live-Modus: cv1_active/cv2_active basieren auf tatsächlich gedrückten Tasten
        cv.setVoltage(0, voltage_ch1);
        cv.setVoltage(1, voltage_ch2);
        updateVCOGates(cv1_active, cv2_active);
    }
    
    // Time-Limit Check
    if(sb1.isRecording() && sb1.timeLimitReached())
    {
        sb1.stopRecord();
        Serial.printf("\n\r[SEQ1] Time limit reached! Recording stopped.");
        Serial.printf("\n\r[SEQ1] Final: Steps: %i, Duration: %ims", 
                     sb1.getStepCount(), sb1.getTotalDuration());
    }
    if(sb2.isRecording() && sb2.timeLimitReached())
    {
        sb2.stopRecord();
        Serial.printf("\n\r[SEQ2] Time limit reached! Recording stopped.");
        Serial.printf("\n\r[SEQ2] Final: Steps: %i, Duration: %ims", 
                     sb2.getStepCount(), sb2.getTotalDuration());
    }
}