#include <Arduino.h>
#include <driver/i2s.h>
#include "opus.h"
#include "opus_data.h"
#include "MeyCan.h"


constexpr int RX_PIN = 2;  // CAN_TRANCEIVER_RX_PIN
constexpr int TX_PIN = 3;  // CAN_TRANCEIVER_TX_PIN
bool driver_installed = false;

constexpr i2s_port_t I2S_PORT = I2S_NUM_0;  // I2S-Peripherie-Instanz (0 = erster Controller)
constexpr int I2S_DIN = 9;                  // GPIO-Pin für serielle Datenleitug (Data-Out → DAC)
constexpr int I2S_BCLK = 10;                 // GPIO-Pin für Bit-Clock (BCLK)
constexpr int I2S_LRC = 20;                  // GPIO-Pin für Word-Select / Left-Right-Clock
constexpr int SAMPLE_RATE = 48000;          // Abtastrate in Hz (Opus-Standard: 48 kHz)
constexpr int CHANNELS = 1;                 // Anzahl Audiokanäle (1 = Mono)
constexpr int MAX_FRAME = 5760;             // Maximale Samples pro Opus-Frame (120 ms @ 48 kHz)
          float OUTPUT_GAIN = 0.02f;        // Lautstärkeskalierung vor I2S-Ausgabe (0.0–1.0)

static OpusDecoder *dec = nullptr;
static int16_t pcm[MAX_FRAME];
static int16_t stereo[MAX_FRAME * 2];

static void i2s_setup() {
  i2s_config_t cfg = {};
  cfg.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
  cfg.sample_rate = SAMPLE_RATE;
  cfg.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
  cfg.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT;
  cfg.communication_format = I2S_COMM_FORMAT_STAND_I2S;
  cfg.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1;
  cfg.dma_buf_count = 8;
  cfg.dma_buf_len = 512;
  cfg.tx_desc_auto_clear = true;

  i2s_pin_config_t pins = {};
  pins.bck_io_num = I2S_BCLK;
  pins.ws_io_num = I2S_LRC;
  pins.data_out_num = I2S_DIN;
  pins.data_in_num = I2S_PIN_NO_CHANGE;

  i2s_driver_install(I2S_PORT, &cfg, 0, nullptr);
  i2s_set_pin(I2S_PORT, &pins);
  i2s_zero_dma_buffer(I2S_PORT);
}

static void decode_packet(const uint8_t *pkt, size_t len) {
  int samples = opus_decode(dec, pkt, (opus_int32)len, pcm, MAX_FRAME, 0);
  if (samples <= 0) return;

  // Mono → Stereo mit Gain
  for (int i = 0; i < samples; i++) {
    int16_t s = (int16_t)(pcm[i] * OUTPUT_GAIN);
    stereo[i * 2] = s;
    stereo[i * 2 + 1] = s;
  }

  size_t written;
  i2s_write(I2S_PORT, stereo, samples * 2 * sizeof(int16_t), &written, portMAX_DELAY);
   CheckMeyPinsTriggered();
}

void DebugBlink(int d) {
  pinMode(20, OUTPUT);
  while (true) {

    digitalWrite(20, HIGH);
    delay(d);
    digitalWrite(20, LOW);
    delay(d);
  }
}

void ConfigureAndSetupMeyCan() {
  // Initialize configuration structures using macro initializers
  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TX_PIN, (gpio_num_t)RX_PIN, TWAI_MODE_NORMAL);
  twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();  //Look in the api-reference for other speed sets.
  twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
    Serial.println("Driver installed");
  } else {
    DebugBlink(100);
    return;
  }

  esp_err_t e = twai_start();
  // Start TWAI driver
  if (e == ESP_OK) {
    driver_installed = true;
  } else {
    DebugBlink(500);
    return;
  }

  SetDevicedId(0x09, 0x09);

  SetMeyPin(1, 5);
  SetMeyPin(2, 6);
  SetMeyPin(3, 7);
  SetMeyPin(4, 8);

  SetupMeyCan(8, 1, 3);

}

// Ogg-Container-Format:
//   Der Stream besteht aus aufeinanderfolgenden "Pages" (Seiten).
//   Jede Page beginnt mit dem 4-Byte-Capture-Pattern "OggS".
//   Der Page-Header ist immer mindestens 27 Bytes lang:
//     Byte  0– 3: "OggS" (Capture Pattern)
//     Byte  4    : Stream-Struktur-Version (immer 0)
//     Byte  5    : Header-Type-Flag (0=normal, 1=continued, 2=first, 4=last)
//     Byte  6–13 : Granule Position (Zeitstempel, 8 Bytes)
//     Byte 14–17 : Stream Serial Number
//     Byte 18–21 : Page Sequence Number
//     Byte 22–25 : CRC Checksum
//     Byte 26    : Anzahl der Segmente (num_segs)
//   Danach folgt die Segment-Tabelle (num_segs Bytes),
//   und anschließend die eigentlichen Nutzdaten.
//
//   Ogg-Lacing-Regel für Paketgrenzen:
//     Jeder Eintrag in der Segment-Tabelle gibt die Größe eines Segments an (0–255).
//     Ist ein Segment genau 255 Bytes → das Paket geht weiter (nächstes Segment gehört dazu).
//     Ist ein Segment < 255 Bytes    → das Paket endet hier.
//   So können Pakete größer als 255 Bytes über mehrere Segmente verteilt sein.
//
//   Bei Opus in Ogg sind die ersten zwei Pages immer Metadaten:
//     Page 0: OpusHead  (Samplerate, Kanalanzahl, Preskip …)
//     Page 1: OpusTags  (Künstler, Titel, Encoder …)
//   Ab Page 2 folgen die eigentlichen Audio-Pakete (je ~20 ms Opus-Frame).
static void play_ogg_opus(const uint8_t *buf, size_t buf_len) {
  size_t pos = 0;  // aktuelle Leseposition im Buffer
  int page_n = 0;  // Page-Zähler (0 und 1 sind Header-Pages)

  while (pos + 27 <= buf_len) {

    // --- 1. Capture Pattern "OggS" suchen ---
    // Falls pos nicht direkt auf eine Page zeigt, byte-weise vorwärts suchen.
    if (!(buf[pos] == 'O' && buf[pos + 1] == 'g' && buf[pos + 2] == 'g' && buf[pos + 3] == 'S')) {
      pos++;
      continue;
    }

    // --- 2. Page-Header lesen ---
    uint8_t num_segs = buf[pos + 26];      // Anzahl Segmente aus Byte 26
    size_t hdr_end = pos + 27 + num_segs;  // Ende des Headers (= Beginn der Nutzdaten)
    if (hdr_end > buf_len) break;

    // Gesamtgröße der Nutzdaten = Summe aller Segmentlängen
    size_t data_size = 0;
    for (int s = 0; s < num_segs; s++) data_size += buf[pos + 27 + s];

    // --- 3. Audio-Pages verarbeiten (Page 0+1 sind Metadaten → überspringen) ---
    if (page_n >= 2) {
      size_t pkt_start = hdr_end;  // Startposition des aktuellen Pakets
      size_t pkt_size = 0;         // akkumulierte Paketgröße über Segmente

      for (int s = 0; s < num_segs; s++) {
        uint8_t seg = buf[pos + 27 + s];  // Größe dieses Segments
        pkt_size += seg;

        if (seg < 255) {
          // Paketende erreicht → vollständiges Opus-Paket dekodieren und ausgeben
          decode_packet(buf + pkt_start, pkt_size);
          pkt_start += pkt_size;  // nächstes Paket beginnt direkt dahinter
          pkt_size = 0;
        }
        // seg == 255 → Paket geht im nächsten Segment weiter (Lacing)
      }
    }

    // --- 4. Zur nächsten Page springen ---
    pos = hdr_end + data_size;
    page_n++;
  }
}

int changeVolume = 0;
static void canTaskFunc(void *) {
  twai_message_t frame;
  for (;;) {
    // blockiert ohne CPU-Last; der interne TWAI-Interrupt weckt den Task sobald ein Frame ankommt
    if (twai_receive(&frame, portMAX_DELAY) == ESP_OK) {
      HandleFrame(&frame);

      if (changeVolume > 20)
        changeVolume = 0;

      OUTPUT_GAIN = 0.1f + changeVolume*0.1f;
      changeVolume++;
    }
  }
}

void setup() {
  Serial.begin(115200);
  i2s_setup();

  // Explicit GND for LED and Input
  pinMode(21, OUTPUT);
  digitalWrite(21, LOW);

  int err;
  dec = opus_decoder_create(SAMPLE_RATE, CHANNELS, &err);
  if (err != OPUS_OK) {
    Serial.printf("opus_decoder_create failed: %d\n", err);
    return;
  }
  Serial.println("Opus playback started");
  ConfigureAndSetupMeyCan();

  xTaskCreate(canTaskFunc, "can", 4096, nullptr, 5, nullptr);
}

void loop() {
  play_ogg_opus(music_opus, music_opus_len);
  opus_decoder_ctl(dec, OPUS_RESET_STATE);
}