/*
*   Analoger Audiosynthesizer mit digitaler Steuereinheit
*   Firmware-Code für die digitale Einheit
*   Autor: Erik Tóth
*/
#include "FIRMWARE_DEF.h"
#include "FIRMWARE.h"

// Calibration table for optimal note accurarcy
const uint16_t NOTE_MV[25] = {
      64,  140,  216,  293,  369,
     445,  521,  597,  673,  750,
     826,  902,  978, 1054, 1131,
    1207, 1283, 1359, 1435, 1511,
    1588, 1664, 1740, 1816, 1892,
};
#define HLFSTEP(n) NOTE_MV[n]

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

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] = {
    HLFSTEP(0),  HLFSTEP(1),  HLFSTEP(2),  HLFSTEP(3),  HLFSTEP(4),
    HLFSTEP(5),  HLFSTEP(6),  HLFSTEP(7),  HLFSTEP(8),  HLFSTEP(9),
    HLFSTEP(10), HLFSTEP(11), HLFSTEP(12), HLFSTEP(13), HLFSTEP(14),
    HLFSTEP(15), HLFSTEP(16), HLFSTEP(17), HLFSTEP(18), HLFSTEP(19),
    HLFSTEP(20), HLFSTEP(21), HLFSTEP(22), HLFSTEP(23), HLFSTEP(24)
};

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

// SB1 -> VCO1 (CV-Channel 0), SB2 -> VCO2 (CV-Channel 1)
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;

// Separate last-voltage tracking per sequencer
static uint16_t sb1_last_voltage_ch1 = 0xFFFF;
static uint16_t sb1_last_voltage_ch2 = 0xFFFF;
static uint16_t sb2_last_voltage_ch1 = 0xFFFF;
static uint16_t sb2_last_voltage_ch2 = 0xFFFF;

bool readButton(byte pin, ButtonState &state)
{
    bool reading = digitalRead(pin) == HIGH;
    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_PULLDOWN);
    pinMode(PIN_SB_1_PLAY, INPUT_PULLDOWN);
    pinMode(PIN_SB_2_REC, INPUT_PULLDOWN);
    pinMode(PIN_SB_2_PLAY, INPUT_PULLDOWN);
    pinMode(PIN_B_METRONOME, INPUT_PULLDOWN);
    
    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()
{
    if(readButton(PIN_SB_1_REC, btn_sb1_rec))
    {
        if(sb1.isRecording())
        {
            sb1.stopRecord();
            Serial.printf("\n\r[SEQ1->VCO1] Recording stopped. Steps: %i, Duration: %ims", 
                         sb1.getStepCount(), sb1.getTotalDuration());
        }
        else
        {
            if(sb1.isPlaying()) sb1.stopPlay();
            sb1.startRecord();
            sb1_last_voltage_ch1 = 0xFFFF;
            sb1_last_voltage_ch2 = 0xFFFF;
            Serial.printf("\n\r[SEQ1->VCO1] Recording started...");
        }
    }
    
    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->VCO1] 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->VCO1] Loop activated");
        }
        else
        {
            sb1.stopPlay();
            seq1_loop_active = false;
            Serial.printf("\n\r[SEQ1->VCO1] Playback stopped");
        }
    }
    
    if(readButton(PIN_SB_2_REC, btn_sb2_rec))
    {
        if(sb2.isRecording())
        {
            sb2.stopRecord();
            Serial.printf("\n\r[SEQ2->VCO2] Recording stopped. Steps: %i, Duration: %ims", 
                         sb2.getStepCount(), sb2.getTotalDuration());
        }
        else
        {
            if(sb2.isPlaying()) sb2.stopPlay();
            sb2.startRecord();
            sb2_last_voltage_ch1 = 0xFFFF;
            sb2_last_voltage_ch2 = 0xFFFF;
            Serial.printf("\n\r[SEQ2->VCO2] Recording started...");
        }
    }
    
    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->VCO2] 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->VCO2] Loop activated");
        }
        else
        {
            sb2.stopPlay();
            seq2_loop_active = false;
            Serial.printf("\n\r[SEQ2->VCO2] 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();
    
    static unsigned long last_bpm_read = 0;
    if((now - last_bpm_read) > 100)
    {
        int adc_value = analogRead(PIN_BPM);
        current_bpm = map(adc_value, 0, 4095, 40, 240);
        last_bpm_read = now;
    }
    
    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);
            metronome_led_on = false;
        }
    }
    
    if(!metronome_enabled) return;
    
    unsigned long beat_interval = 60000UL / current_bpm;
    
    if((now - last_beat_time) >= beat_interval)
    {
        digitalWrite(PIN_L_METRONOME, LOW);
        metronome_led_on = true;
        last_beat_time = now;
        last_pulse_end_time = now + 50;
    }
    
    if(metronome_led_on && (now >= last_pulse_end_time))
    {
        digitalWrite(PIN_L_METRONOME, HIGH);
        metronome_led_on = false;
    }
}

void updateRecordingLED()
{
    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=== DUAL SEQUENCER: SB1->VCO1 | SB2->VCO2 ===");
    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=== System Started ===");
    Serial.printf("\n\rMapping:");
    Serial.printf("\n\r  SB1 -> VCO1 (CV-Ch 0) | SB2 -> VCO2 (CV-Ch 1)");
    Serial.printf("\n\rManual fallback:");
    Serial.printf("\n\r  SB1 playing, SB2 idle  -> VCO2 manual (Queue[0])");
    Serial.printf("\n\r  SB2 playing, SB1 idle  -> VCO1 manual (Queue[0])");
    Serial.printf("\n\r  Both idle              -> VCO1=Queue[0], VCO2=Queue[1]");
    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->VCO1: Rec=%d, Play=%d, Steps=%d", 
                     sb1.isRecording(), sb1.isPlaying(), sb1.getStepCount());
        Serial.printf("\n\r[DEBUG] SB2->VCO2: 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 
    keyboard.update();
    handleSequencerButtons();
    updateMetronome();
    updateRecordingLED();
    
    sb1.update();
    sb2.update();
    
    // KEYBOARD INPUT
    int n = keyboard.getQueueLength();
    
    // Key 0 -> wird als manueller Eingang für den jeweils freien VCO genutzt
    uint16_t manual_voltage_0 = 0;
    uint16_t manual_voltage_1 = 0;
    bool manual_active_0 = false;
    bool manual_active_1 = false;
    
    if(n > 0)
    {
        Key k = keyboard.getQueue(0);
        if(!isNotKey(k))
        {
            manual_voltage_0 = keyToVoltage[k.row * N_KEYBOARD_COL + k.col];
            manual_active_0 = true;
        }
    }
    
    if(n > 1)
    {
        Key k = keyboard.getQueue(1);
        if(!isNotKey(k))
        {
            manual_voltage_1 = keyToVoltage[k.row * N_KEYBOARD_COL + k.col];
            manual_active_1 = true;
        }
    }

    // ===== RECORDING =====
    // SB1 nimmt immer ch1=manual_voltage_0 / ch2=manual_voltage_1 auf
    // (SB1 ist für VCO1 zuständig, nutzt den vollen Keyboard-Input)
    if(sb1.isRecording())
    {
        bool changed = (manual_voltage_0 != sb1_last_voltage_ch1) ||
                       (manual_voltage_1 != sb1_last_voltage_ch2);
        if(changed)
        {
            sb1.addStep(manual_voltage_0, manual_voltage_1);
            sb1_last_voltage_ch1 = manual_voltage_0;
            sb1_last_voltage_ch2 = manual_voltage_1;
        }
    }
    
    // SB2 nimmt ebenfalls den vollen Keyboard-Input auf
    if(sb2.isRecording())
    {
        bool changed = (manual_voltage_0 != sb2_last_voltage_ch1) ||
                       (manual_voltage_1 != sb2_last_voltage_ch2);
        if(changed)
        {
            sb2.addStep(manual_voltage_0, manual_voltage_1);
            sb2_last_voltage_ch1 = manual_voltage_0;
            sb2_last_voltage_ch2 = manual_voltage_1;
        }
    }

    // ===== CV OUTPUT & VCO GATES =====
    //
    //  SB1 state   | SB2 state   | VCO1 (ch 0)         | VCO2 (ch 1)
    //  ------------|-------------|---------------------|----------------------
    //  playing     | playing     | SB1 seq voltage     | SB2 seq voltage
    //  playing     | recording   | SB1 seq voltage     | live manual Queue[0]
    //  playing     | idle        | SB1 seq voltage     | live manual Queue[0]
    //  idle        | playing     | live manual Queue[0]| SB2 seq voltage
    //  idle        | recording   | live manual Queue[0]| live manual Queue[0]
    //  idle        | idle        | live manual Queue[0]| live manual Queue[1]

    bool sb1_playing   = sb1.isPlaying();
    bool sb1_recording = sb1.isRecording();
    bool sb2_playing   = sb2.isPlaying();
    bool sb2_recording = sb2.isRecording();

    uint16_t out_vco1 = 0;
    uint16_t out_vco2 = 0;
    bool gate_vco1 = false;
    bool gate_vco2 = false;

    // VCO1
    if(sb1_playing)
    {
        // SB1 Sequenz läuft -> Sequenz-Ausgabe
        out_vco1  = sb1.getCurrentVoltageCh1();
        gate_vco1 = sb1.isCurrentStepActive();
    }
    else if(sb1_recording)
    {
        // SB1 nimmt auf -> Live-Ausgabe damit man hört was man spielt
        out_vco1  = manual_voltage_0;
        gate_vco1 = manual_active_0;
    }
    else
    {
        // SB1 idle -> manuell
        out_vco1  = manual_voltage_0;
        gate_vco1 = manual_active_0;
    }

    // VCO2
    if(sb2_playing)
    {
        // SB2 Sequenz läuft -> Sequenz-Ausgabe
        out_vco2  = sb2.getCurrentVoltageCh1();
        gate_vco2 = sb2.isCurrentStepActive();
    }
    else if(sb2_recording)
    {
        // SB2 nimmt auf -> Live-Ausgabe damit man hört was man spielt
        out_vco2  = manual_voltage_0;
        gate_vco2 = manual_active_0;
        gate_vco1 = false;
    }
    else if(sb1_playing)
    {
        // SB1 läuft, SB2 idle -> VCO2 manuell mit Queue[0]
        out_vco2  = manual_voltage_0;
        gate_vco2 = manual_active_0;
    }
    else
    {
        // Beide idle -> VCO2 bekommt Queue[1]
        out_vco2  = manual_voltage_1;
        gate_vco2 = manual_active_1;
    }
    
    cv.setVoltage(0, out_vco1);   // CH_A -> VCO1
    cv.setVoltage(1, out_vco2);   // CH_B -> VCO2
    
    digitalWrite(PIN_VCO1_EN, gate_vco1 ? HIGH : LOW);
    digitalWrite(PIN_VCO2_EN, gate_vco2 ? HIGH : LOW);
    
    // TIME-LIMIT CHECK
    if(sb1.isRecording() && sb1.timeLimitReached())
    {
        sb1.stopRecord();
        Serial.printf("\n\r[SEQ1->VCO1] Time limit reached! Recording stopped.");
        Serial.printf("\n\r[SEQ1->VCO1] Final: Steps: %i, Duration: %ims", 
                     sb1.getStepCount(), sb1.getTotalDuration());
    }
    if(sb2.isRecording() && sb2.timeLimitReached())
    {
        sb2.stopRecord();
        Serial.printf("\n\r[SEQ2->VCO2] Time limit reached! Recording stopped.");
        Serial.printf("\n\r[SEQ2->VCO2] Final: Steps: %i, Duration: %ims", 
                     sb2.getStepCount(), sb2.getTotalDuration());
    }
}