#include "expression_pedal.h"
#include "midi_transport.h"
#include <Arduino.h>

ExpressionPedal::ExpressionPedal(uint8_t adc_pin)
    : adc_pin(adc_pin)
    , current_value(0)
    , last_sent_value(255)
    , midi_channel(1)
    , midi_cc(4)
    , last_read_time(0)
    , current_raw(0)
    , smoothed_raw(0)
    , cal_min(36)
    , cal_max(1100)
{
}

void ExpressionPedal::begin() {
    Serial.printf("[EXP] Expression pedal on ADC1_CH3 (GPIO%d)\n", adc_pin);
    analogReadResolution(12);
    pinMode(adc_pin, INPUT);
    delay(10);
    current_raw = analogRead(adc_pin);
    smoothed_raw = current_raw;
    current_value = adc_to_midi(smoothed_raw);
    last_sent_value = current_value;
    Serial.printf("[EXP] Starting: raw ADC=%d -> MIDI=%d (cal: %d-%d)\n",
                  current_raw, current_value, cal_min, cal_max);
    Serial.printf("[EXP] Move pedal full range to see raw values\n");
}

void ExpressionPedal::update(UsbMidiTransport& midi) {
    uint32_t now = millis();
    if (now - last_read_time < READ_INTERVAL_MS) return;
    last_read_time = now;

    current_raw = analogRead(adc_pin);
    smoothed_raw = (smoothed_raw * 7 + current_raw + 4) / 8;
    uint8_t new_value = adc_to_midi(smoothed_raw);
    current_value = new_value;

    // Proportional catch-up: send midpoint rounded up each update.
    // Rejects ±1 stationary jitter but converges to exact value on movement
    // (e.g., 0→127 converges in ~35ms).
    if (current_value > last_sent_value + 1) {
        last_sent_value = (last_sent_value + current_value + 1) / 2;
        midi.send_cc(midi_channel, midi_cc, last_sent_value);
    } else if (current_value < last_sent_value - 1) {
        last_sent_value = (last_sent_value + current_value) / 2;
        midi.send_cc(midi_channel, midi_cc, last_sent_value);
    }
}

void ExpressionPedal::set_cal_min() {
    cal_min = current_raw;
    if (cal_min >= cal_max) cal_max = cal_min + 1;
    Serial.printf("[EXP] cal_min set to %d (ADC raw)\n", cal_min);
}

void ExpressionPedal::set_cal_max() {
    cal_max = current_raw;
    if (cal_max <= cal_min) cal_min = cal_max - 1;
    Serial.printf("[EXP] cal_max set to %d (ADC raw)\n", cal_max);
}

uint8_t ExpressionPedal::adc_to_midi(uint16_t adc_value) {
    if (adc_value <= cal_min) return 0;
    if (adc_value >= cal_max) return 127;
    uint32_t span = cal_max - cal_min;
    uint32_t offset = adc_value - cal_min;
    return (uint8_t)(offset * 127 / span);
}