#include "u8g2.h"
 #include "u8g2_esp32_hal.h"
 #include "BT_font.c"
+#include <algorithm>
 
 #define TIMER_DIVIDER 16
 #define TIMER_SCALE (TIMER_BASE_CLK / TIMER_DIVIDER)
 #define EXT_DAC_DATA_PIN GPIO_NUM_17
 
 // -------------- Flags --------------
-enum StatusBM {
+enum StatusBM
+{
     BT = 1,
     PLAY = 2,
     STOP = 4,
     PAUSE = 8,
     METADATA = 16
 };
-enum eventBM {
+enum eventBM
+{
     DISPLAY = 1,
     INPUT = 2,
 };
 
 u8g2_t u8g2;
 u8g2_esp32_hal_t u8g2_esp32_hal;
-xQueueHandle event_queue;
 
 // -------------- Global tracking vars --------------
-char* titleText = (char*) "";
-char* artistText = (char*) "";
-uint8_t titleWidth = 0; 
+char *titleText = (char *)"";
+char *artistText = (char *)"";
+uint8_t titleWidth = 0;
 uint8_t artistWidth = 0;
 
 uint8_t displayStatus = 0;
 // --------------------------------------------------
 
 // -------------- Function definitions --------------
-void update_display(al_event_cb_t, al_event_cb_param_t*);
+void update_display(al_event_cb_t, al_event_cb_param_t *);
 void IRAM_ATTR draw();
-bool IRAM_ATTR inp  ut_timer_cb(void* args);
-bool IRAM_ATTR display_timer_cb(void* args);
+bool IRAM_ATTR input_timer_cb(void *args);
+bool IRAM_ATTR display_timer_cb(void *args);
 
-void pot1_update_cb(void);
-void pot2_update_cb(void);
-void pot3_update_cb(void);
-void pot4_update_cb(void);
+void pot_volume_update_cb(float);
+void pot_treb_update_cb(float);
+void pot_mid_update_cb(float);
+void pot_bass_update_cb(float);
 
 void btn1_update_cb(void);
 void btn2_update_cb(void);
 // -------------- Object definitions --------------
 Audiolib Audiosource = Audiolib("HESA PETTER", &update_display);
 
-Potentiometer* pot1 = new Potentiometer(ADC_UNIT_1 ,ADC_CHANNEL_0, 70, 1010, &pot1_update_cb);
-Potentiometer* pot2 = new Potentiometer(ADC_UNIT_1 ,ADC_CHANNEL_0, 41, 988, &pot2_update_cb);
-Potentiometer* pot3 = new Potentiometer(ADC_UNIT_1 ,ADC_CHANNEL_0, 38, 988, &pot3_update_cb);
-Potentiometer* pot4 = new Potentiometer(ADC_UNIT_1 ,ADC_CHANNEL_0, 29, 990, &pot4_update_cb);
+Potentiometer *pot_volume = new Potentiometer(ADC_UNIT_1, ADC_CHANNEL_0, 1, &pot_volume_update_cb);
+Potentiometer *pot_treb = new Potentiometer(ADC_UNIT_1, ADC_CHANNEL_0, 1, &pot_treb_update_cb);
+Potentiometer *pot_mid = new Potentiometer(ADC_UNIT_1, ADC_CHANNEL_3, 1, &pot_mid_update_cb);
+Potentiometer *pot_bass = new Potentiometer(ADC_UNIT_1, ADC_CHANNEL_6, 1, &pot_bass_update_cb);
 
-Button* btn1 = new Button(BTN1_PIN, &btn1_update_cb);
-Button* btn2 = new Button(BTN2_PIN, &btn2_update_cb);
-Button* btn3 = new Button(BTN3_PIN, &btn3_update_cb);
+Button *btn1 = new Button(BTN1_PIN, &btn1_update_cb);
+Button *btn2 = new Button(BTN2_PIN, &btn2_update_cb);
+Button *btn3 = new Button(BTN3_PIN, &btn3_update_cb);
 
-CombinedChannelFilter* highshelf_filter = new CombinedChannelFilter(new Filter(HIGHSHELF, 2000, 44100, 0.8, 0), new Filter(HIGHSHELF, 2000, 44100, 0.8, 0));
-CombinedChannelFilter* lowshelf_filter = new CombinedChannelFilter(new Filter(LOWSHELF, 250, 44100, 0.8, 0), new Filter(LOWSHELF, 250, 44100, 0.8, 0));
-//CombinedChannelFilter* highpass_filter = new CombinedChannelFilter(new Filter(LOWPASS, 8000, 44100, 0.75, 0), new Filter(LOWPASS, 8000, 44100, 0.75, 0));
-//CombinedChannelFilter* lowpass_filter = new CombinedChannelFilter(new Filter(HIGHPASS, 60, 44100, 0.75, 0), new Filter(HIGHPASS, 60, 44100, 0.75, 0));
-CombinedChannelFilter* peak_filter = new CombinedChannelFilter(new Filter(PEAK, 700, 44100, 0.8, 0), new Filter(PEAK, 700, 44100, 0.8, 0));
+CombinedChannelFilter *highshelf_filter = new CombinedChannelFilter(new Filter(HIGHSHELF, 2000, 44100, 0.8, 0), new Filter(HIGHSHELF, 2000, 44100, 0.8, 0));
+CombinedChannelFilter *lowshelf_filter = new CombinedChannelFilter(new Filter(LOWSHELF, 250, 44100, 0.8, 0), new Filter(LOWSHELF, 250, 44100, 0.8, 0));
+CombinedChannelFilter* highpass_filter = new CombinedChannelFilter(new Filter(LOWPASS, 10000, 44100, 0.75, 0), new Filter(LOWPASS, 10000, 44100, 0.75, 0));
+CombinedChannelFilter* lowpass_filter = new CombinedChannelFilter(new Filter(HIGHPASS, 50, 44100, 0.75, 0), new Filter(HIGHPASS, 50, 44100, 0.75, 0));
+CombinedChannelFilter *peak_filter = new CombinedChannelFilter(new Filter(PEAK, 700, 44100, 0.8, 0), new Filter(PEAK, 700, 44100, 0.8, 0));
 // ------------------------------------------------
 
-extern "C" {
-    void app_main(void){
+extern "C"
+{
+    void app_main(void)
+    {
 
         // -------------- U8G2 setup --------------
         u8g2_esp32_hal.sda = DISPLAY_SDA_PIN;
         u8g2_SetFont(&u8g2, BT_FONT);
         // ----------------------------------------
 
-        event_queue = xQueueCreate(10, 1);
-
         // -------------- Audiosource setup --------------
         Audiosource.set_I2S(EXT_DAC_BCLK_PIN, EXT_DAC_WSEL_PIN, EXT_DAC_DATA_PIN);
-        //Audiosource.add_combined_filter(highpass_filter);
-        Audiosource.add_combined_filter(lowshelf_filter);  
-        Audiosource.add_combined_filter(peak_filter); 
+        Audiosource.add_combined_filter(highpass_filter);
+        Audiosource.add_combined_filter(lowshelf_filter);
+        Audiosource.add_combined_filter(peak_filter);
         Audiosource.add_combined_filter(highshelf_filter);
-        //Audiosource.add_combined_filter(lowpass_filter);
+        Audiosource.add_combined_filter(lowpass_filter);
         Audiosource.start();
         // -----------------------------------------------
 
-        // -------------- Timer setup --------------
-        timer_init(TIMER_GROUP_0, INPUT_TIMER, &timer_config);
-        timer_set_counter_value(TIMER_GROUP_0, INPUT_TIMER, 0);
-        timer_set_alarm_value(TIMER_GROUP_0, INPUT_TIMER, INPUT_INTERVAL_S * TIMER_SCALE);
-        timer_enable_intr(TIMER_GROUP_0, INPUT_TIMER);
-        timer_isr_callback_add(TIMER_GROUP_0, INPUT_TIMER, &input_timer_cb, nullptr, 0);
-    
-        timer_init(TIMER_GROUP_0, DISPLAY_TIMER, &timer_config);
-        timer_set_counter_value(TIMER_GROUP_0, DISPLAY_TIMER, 0);
-        timer_set_alarm_value(TIMER_GROUP_0, DISPLAY_TIMER, DISPLAY_INTERVAL_S * TIMER_SCALE);
-        timer_enable_intr(TIMER_GROUP_0, DISPLAY_TIMER);
-        timer_isr_callback_add(TIMER_GROUP_0, DISPLAY_TIMER, &display_timer_cb, nullptr, 0);
-
-        timer_start(TIMER_GROUP_0, INPUT_TIMER);
-        timer_start(TIMER_GROUP_0, DISPLAY_TIMER);
-        // -----------------------------------------
-
         // Main loop
-        while (true) {
-            xQueueReceive(event_queue, &event, portMAX_DELAY);
-            if (event & DISPLAY) {
-                draw();
-            }
-            if (event & INPUT) {
-                /*
-                pot1->update();
-                pot2->update();
-                pot3->update();
-                pot4->update();
-
-                btn1->update();
-                btn2->update();
-                btn3->update();
-                */
-                if (btn1->getPressedSingle()){
-                    Audiosource.previous();
-                }
-                if (btn2->getPressedSingle()) {
-                    if (displayStatus & PLAY) {
-                        Audiosource.pause();
-                    }
-                    else if (displayStatus & (PAUSE | STOP)){
-                        Audiosource.play();
-                    }
-                }
-                if (btn3->getPressedSingle()) {
-                    Audiosource.next();
-                }
-            }
+        while (true)
+        {
+            draw();
+
+            pot_volume->update();
+            pot_treb->update();
+            pot_mid->update();
+            pot_bass->update();
+
+            btn1->update();
+            btn2->update();
+            btn3->update();
+
+            vTaskDelay(1);
         }
     }
 }
 
-void draw() {
-    if (displayStatus & BT) {
-        u8g2_DrawGlyph(&u8g2, 0, SCREEN_HEIGHT/2, 127);
+void draw()
+{
+    if (displayStatus & BT)
+    {
+        u8g2_DrawGlyph(&u8g2, 0, SCREEN_HEIGHT / 2, 127);
 
-        if (displayStatus & STOP) {
-            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT/2, 129);
+        if (displayStatus & STOP)
+        {
+            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT / 2, 129);
         }
-        else if (displayStatus & PAUSE) {
-            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT/2, 130);
+        else if (displayStatus & PAUSE)
+        {
+            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT / 2, 130);
         }
-        else if (displayStatus & PLAY) {
-            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT/2, 128);
+        else if (displayStatus & PLAY)
+        {
+            u8g2_DrawGlyph(&u8g2, 12, SCREEN_HEIGHT / 2, 128);
         }
-        
-        if (displayStatus & METADATA) {
-            u8g2_DrawStr(&u8g2, 23, SCREEN_HEIGHT/2, artistText);
+
+        if (displayStatus & METADATA)
+        {
+            u8g2_DrawStr(&u8g2, 23, SCREEN_HEIGHT / 2, artistText);
             u8g2_DrawStr(&u8g2, title_x, SCREEN_HEIGHT, titleText);
             title_x -= 1;
-            if (title_x == -SCREEN_WIDTH) title_x = 0;
+            if (title_x + titleWidth <= SCREEN_WIDTH)
+                title_x = 0;
+
+            /*
+            artist_x -= 1;
+            if (artist_x + artistWidth + 23 <= SCREEN_WIDTH)
+                artist_x = 0;
+            */
         }
-        else {
-            u8g2_DrawStr(&u8g2, 23, SCREEN_HEIGHT/2, "Connected!");
+        else
+        {
+            u8g2_DrawStr(&u8g2, 23, SCREEN_HEIGHT / 2, "Connected!");
         }
     }
-    else {
-        u8g2_DrawStr(&u8g2, 0, SCREEN_HEIGHT/2, "Disconnected!");
-        u8g2_DrawStr(&u8g2, 0, SCREEN_HEIGHT, "Connect Device!");  
+    else
+    {
+        u8g2_DrawStr(&u8g2, 0, SCREEN_HEIGHT / 2, "Disconnected!");
+        u8g2_DrawStr(&u8g2, 0, SCREEN_HEIGHT, "Connect Device!");
     }
-
     u8g2_SendBuffer(&u8g2);
     u8g2_ClearBuffer(&u8g2);
 }
 
-void update_display(al_event_cb_t event, al_event_cb_param_t* param) {
-    switch (event) {
+void update_display(al_event_cb_t event, al_event_cb_param_t *param)
+{
+    switch (event)
+    {
     case AL_CONNECTED:
         displayStatus = BT | STOP;
         printf("AL, Connected\n");
         break;
-    
+
     case AL_DISCONNECTED:
         displayStatus = 0;
         printf("AL, Disconnected\n");
         break;
-    
+
     case AL_PLAYING:
         displayStatus = BT | PLAY | METADATA;
         printf("AL, Playing\n");
     case AL_META_UPDATE:
         titleText = param->metadata.title;
         artistText = param->metadata.artist;
-        if ((*artistText == '\0') || (*titleText == '\0')) {
+        if ((*artistText == '\0') || (*titleText == '\0'))
+        {
             displayStatus = BT | STOP;
         }
-        else {
+        else
+        {
             displayStatus |= METADATA;
         }
         artistWidth = u8g2_GetStrWidth(&u8g2, artistText);
     }
 }
 
-bool input_timer_cb(void* args) {
+bool input_timer_cb(void *args)
+{
     int8_t msg = INPUT;
     BaseType_t high_task_awoken = pdFALSE;
     xQueueSendFromISR(event_queue, &msg, &high_task_awoken);
     return high_task_awoken == pdTRUE;
 }
 
-bool display_timer_cb(void* args) {
+bool display_timer_cb(void *args)
+{
     int8_t msg = DISPLAY;
     BaseType_t high_task_awoken = pdFALSE;
     xQueueSendFromISR(event_queue, &msg, &high_task_awoken);
     return high_task_awoken == pdTRUE;
 }
 
-void pot1_update_cb() {
-    printf("Pot 1 changed\n");
+void pot_volume_update_cb(float val)
+{
+    printf("Volume changed %.2f\n", val);
+    peak_filter->change_volume(val/100);
 }
-void pot2_update_cb() {
-    printf("Pot 2 changed\n");
+void pot_treb_update_cb(float val)
+{
+    printf("Treb changed %.2f\n", val);
+    highshelf_filter->update((val - 50) / 4);
 }
-void pot3_update_cb() {
-    printf("Pot 3 changed\n");
+void pot_mid_update_cb(float val)
+{
+    printf("Mid changed %.2f\n", val);
+    peak_filter->update((val-50) / 4);
 }
-void pot4_update_cb() {
-    printf("Pot 4 changed\n");
+void pot_bass_update_cb(float val)
+{
+    printf("Bass changed %.2f\n", val);
+    lowshelf_filter->update((val-50) / 4);
 }
 
-
-void btn1_update_cb() {
-    Audiosource.previous();
+void btn1_update_cb()
+{
+    Audiosource.next();
 }
-void btn2_update_cb() {
-    if (displayStatus & PLAY) {
+void btn2_update_cb()
+{
+    if (displayStatus & PLAY)
+    {
         Audiosource.pause();
     }
-    else if (displayStatus & (PAUSE | STOP)){
+    else if (displayStatus & (PAUSE | STOP))
+    {
         Audiosource.play();
     }
 }
-void btn3_update_cb() {
-    Audiosource.next();
+void btn3_update_cb()
+{
+    Audiosource.previous();
 }
\ No newline at end of file