#include <Arduino.h>
#include <hardware/watchdog.h>
#include <muTimer.h>
#include "pico/multicore.h"
#include <Smoothed.h>


#define SAMPLEPERIODUS 200UL   // Okres 

muTimer Decay = muTimer();
muTimer Decay2 = muTimer();
bool decay_2_in;
bool decay_2_out;

const unsigned long BPM_INTERVAL = 60000;  // 1 minuta
unsigned long beatCount = 0;
unsigned long lastBeatTime = 0;
float bpm = 0;
const int HISTORY_SIZE = 5;  // Liczba ostatnich uderzen do analizy
unsigned long beatIntervals[HISTORY_SIZE] = {0};
int beatIndex = 0;
bool stable = false;  // Flaga czy sygnal ok

//beat led clk
const int LED_PIN = 2;
unsigned long beatInterval;  // Odstęp trig/trig w ms
bool BPM_CLK_LED;
muTimer LEDDecay = muTimer();
muTimer LEDBPM = muTimer();


//vu meter

const int NUM_BARS = 10;  // Liczba VU
float peakLevel = 0;       // Przechowuje (peak hold)
unsigned long peakHoldTime = 500;  // Podtrzymanie poziomu (ms)
unsigned long lastPeakUpdate = 0;  // Ostatni peak hold
unsigned long lastUpdateTime = 0;  // Ostatni czas VU



Smoothed <float> myBPM;


muTimer co_250ms = muTimer();
bool trig_co_250ms;
muTimer co_100ms = muTimer();
bool trig_co_100ms;
muTimer co_1s = muTimer();
bool trig_co_1s;

#define serial_debug
#define RS232_RX 5
#define RS232_TX 4

/*
DEBUG PINY:
DEBUG POM(SWDIO) / CZAR(GND) / ZOL(SCLK)
RS232 CZAR(GND) / POM(TX) / ZOL(RX)
*/

//VARIABLES
unsigned int running_counter; 
unsigned int running_counter_c1;
bool beat_x ; 


///

//
// *20 - 200Hz Single Pole Bandpass IIR Filter
//
float bassFilter(float sample) {
  static float xv[3] = {0.0f, 0.0f, 0.0f};
  static float yv[3] = {0.0f, 0.0f, 0.0f};

  xv[0] = xv[1]; 
  xv[1] = xv[2];
  xv[2] = sample / 3.0f;  //* regulacja pod sile sygnalu
  
  yv[0] = yv[1];
  yv[1] = yv[2];
  yv[2] = (xv[2] - xv[0])
         + (-0.7960060012f * yv[0])
         + (1.7903124146f * yv[1]);
  return yv[2];
}

//
//* 10Hz Single Pole Lowpass IIR Filter
//
float envelopeFilter(float sample) {
  static float xv[2] = {0.0f, 0.0f};
  static float yv[2] = {0.0f, 0.0f};

  xv[0] = xv[1];
  xv[1] = sample / 50.0f;
  
  yv[0] = yv[1];
  yv[1] = (xv[0] + xv[1]) + (0.9875119299f * yv[0]);
  return yv[1];
}

//
//* 1.7 - 3.0Hz Single Pole Bandpass IIR Filter
//
float beatFilter(float sample) {
  static float xv[3] = {0.0f, 0.0f, 0.0f};
  static float yv[3] = {0.0f, 0.0f, 0.0f};

  xv[0] = xv[1];
  xv[1] = xv[2];
  xv[2] = sample / 2.7f;
  
  yv[0] = yv[1];
  yv[1] = yv[2];
  yv[2] = (xv[2] - xv[0])
         + (-0.7169861741f * yv[0])
         + (1.4453653501f * yv[1]);
  return yv[2];
}


///


// SETUP NA RDZENIU 1
void setup1() {
  #ifdef serial_debug
  Serial2.println("1: Port Szeregowy Gotowy!"); 
  #endif

  delay(1000);
}

// PETLA LOOP NA RZENIU 1
void loop1() {
  delay(500);
  running_counter_c1 = running_counter_c1 + 1;
}


///funkcje 

unsigned long getAverageInterval() {
    unsigned long sum = 0;
    for (int i = 0; i < HISTORY_SIZE; i++) {
        sum += beatIntervals[i];
    }
    return sum / HISTORY_SIZE;
}

bool isStable() {
    unsigned long avg = getAverageInterval();
    for (int i = 0; i < HISTORY_SIZE; i++) {
        if (beatIntervals[i] == 0) return false;  // Nie mamy jeszcze....
        if (abs((long)beatIntervals[i] - (long)avg) > 100) return false;  // Maksymalne odchylenie...
    }
    return true;
}




//SETUP -- SETUP -- SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP
void setup() {
    
   //pin i/o
   pinMode(LED_BUILTIN, OUTPUT);
   pinMode(LED_PIN, OUTPUT);
   
   //adc res
   analogReadResolution(12);


  //Serial_DEBUG 
  #ifdef serial_debug
  Serial2.setTX(RS232_TX);
  Serial2.setRX(RS232_RX);
  Serial2.begin(115200); 
  Serial2.println("0: Port Szeregowy Gotowy!"); 
  #endif
  
  //timery
  Decay.delayOffTrigger(1, 100);
  LEDDecay.delayOffTrigger(1, 50);
  co_100ms.cycleTrigger(100);
  co_250ms.cycleTrigger(250);
  co_1s.cycleTrigger(1000);


  //filtry
  myBPM.begin(SMOOTHED_AVERAGE, 50);
//wdt
  watchdog_enable(8000, false);



}//setup end.
//SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP --SETUP -- SETUP 



//LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP 
void loop() {

 ///
  static unsigned int sampleCounter = 0;
  static unsigned long lastSampleTime = 0;
  unsigned long currentTime = micros();

  // Co: 5000Hz (co 200 µs)
  if (currentTime - lastSampleTime < SAMPLEPERIODUS) return;
  lastSampleTime = currentTime;

  // Odczyt A0
  // Offset: dla 12-bit ADC (0-4095) ~ 2048
  float sample = (float)analogRead(A0) - 2048.0f;

  // Basofiltr
  float value = bassFilter(sample);
  if (value < 0) value = -value;

  // Obwiednia
  float envelope = envelopeFilter(value);

  sampleCounter++;
  // Co 200  (~25Hz) wykonujemy filtr beat <-> ustawiamy stan LED
  if (sampleCounter >= 200) {
    float beat = beatFilter(envelope);

    // Odczyt A1 do ustawienia progu
    float thresh = 0.02f * (float)analogRead(A1);

    // Jeżeli wykryty beat > prog, LED
    bool beat_detect;
    if (beat > thresh)
       beat_detect = true;
      //digitalWrite(LED_BUILTIN, HIGH);
    else
     //digitalWrite(LED_BUILTIN, LOW);
     beat_detect = false;
    sampleCounter = 0;

      beat_x = Decay.delayOff(beat_detect, 100);

  }

  digitalWrite(LED_BUILTIN, beat_x);

 ///
   static bool lastState = false;
    
    if (beat_x && !lastState) {  // Wykrycie zbocza narastajacego
        unsigned long currentTime = millis();
        
        if (lastBeatTime > 0) {
            unsigned long interval = currentTime - lastBeatTime;
            beatIntervals[beatIndex] = interval;
            beatIndex = (beatIndex + 1) % HISTORY_SIZE;
            
            if (isStable()) {
                bpm = 60000.0 / getAverageInterval();
                stable = true;
            } else {
                stable = false;
            }
        }
        
        lastBeatTime = currentTime;
    }
    
    lastState = beat_x;
 ///

///
 
 
   
///
 beatInterval = 60000 / bpm;

//LEDDecay.cycleTrigger(beatInterval);
if(beat_x == true){
BPM_CLK_LED =  LEDBPM.delayOff(LEDDecay.cycleTrigger(beatInterval), 10);
}
if(beat_x == false){
BPM_CLK_LED = false;
}

  


 if (beat_x == true and BPM_CLK_LED == true){
decay_2_in = true;
 }
 else{
decay_2_in = false;
 }

decay_2_out =  Decay2.delayOff(decay_2_in, 100);

if(beat_x == false){
decay_2_out = false;
}

digitalWrite(LED_PIN, decay_2_out);

///vu meter

 // Aktualizacja co 50ms bez delay()
    if (currentTime - lastUpdateTime >= 50) {
        lastUpdateTime = currentTime;

        // 1. Odczytaj
        int rawValue = analogRead(A0);

        // 2. Mapuj
        float level = map(rawValue, 2000, 2250, 0, NUM_BARS);

        // 3. Peak Hold
        if (level > peakLevel) {
            peakLevel = level;
            lastPeakUpdate = currentTime;  // Zapisujemy czas aktualizacji
        }

        // 4. Sprawdzam > czas peak hold
        if (currentTime - lastPeakUpdate > peakHoldTime) {
            peakLevel -= 0.2;  // Powolne opadanie peak hold 
            if (peakLevel < 0) peakLevel = 0; // Antyminus
        }

        // 5. Wyświetlenie VU 
        Serial.print("VU: [");
        for (int i = 0; i < NUM_BARS; i++) {
            if (i < level) {
                Serial.print("#");  // Aktualny poziom 
            } else {
                Serial.print(" ");  // Puste miejsca
            }
        }

        // 6. Wyświetlenie Peak
        if (peakLevel >= 1) {
            int peakPos = min(NUM_BARS - 1, (int)peakLevel);
            if (peakPos >= (int)level) {
                Serial.print(" *");  // Jak poza tunning
            }
        }

        Serial.println("]");  // Japierdole z tym.... 
    }
 


//co 100ms
if(co_100ms.cycleTrigger(100)){
   trig_co_100ms = not(trig_co_100ms);
  // digitalWrite(LED_BUILTIN, trig_co_100ms);
  
  myBPM.add(bpm);
}//co 100ms end.

//co 1s
if(co_1s.cycleTrigger(1000)){
   trig_co_1s = not(trig_co_1s);
   running_counter = running_counter + 1;
  
  
  //SERIAL DEBUG SERIAL DEBUG SERIAL DEBUG SERIAL DEBUG
  #ifdef serial_debug
  //Serial2.print("0:running counter: "); 
  //Serial2.print(running_counter);
  //Serial2.print("\n");
  //Serial2.print("1:running counter: "); 
  //Serial2.print(running_counter_c1);
  //Serial2.print("\n");
  
  Serial2.print("Treshold: "); 
  Serial2.print(analogRead(A1));
  Serial2.print("\n");
  Serial2.print("Signal: "); 
  Serial2.print(analogRead(A0));
  Serial2.print("\n");
  Serial2.print("BPM: "); 
  Serial2.print(myBPM.get());
  Serial2.print(" STABLE: "); 
  Serial2.print(stable);
  Serial2.print("\n");
  Serial2.print("INTERVAL: "); 
  Serial2.print(beatInterval);
  Serial2.print("\n");
  
  #endif


}//co 1s end.

//WDT ! WDT ! WDT ! WDT ! WDT ! WDT ! WDT ! WDT !
watchdog_update();
}//loop end.
//LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP - LOOP