/*
 *  @file       task.cpp
 *  Project     Blackstomp Arduino Library
 *  @brief      Blackstomp Library for the Arduino
 *  @author     Hasan Murod
 *  @date       19/11/2020
 *  @license    MIT - Copyright (c) 2020 Hasan Murod
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
  
#include "task.h"
#include "set_module.h" 
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//codec instance
static bool _muteLeftAdcIn = false;
static bool _muteRightAdcIn = false;
bool res = false;

static bool _codecIsReady = false;
static float _outCorrectionGain = 1;
static int _optimizedRange = 2;

 // processor timing variables for system monitor
  unsigned int runningTicks;
  unsigned int usedticks;
  unsigned int availableticks;
  unsigned int availableticks_start;
  unsigned int availableticks_end;
  unsigned int usedticks_start;
  unsigned int usedticks_end;
  unsigned int processedframe;
  unsigned int audiofps;



    // Parameter 1 = number of pixels in strip
    // Parameter 2 = Arduino pin number (most are valid)
    // Parameter 3 = pixel type flags, add together as needed:
    //   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
    //   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
    //   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
    //   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
    //   NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)

    //  Create instance object of Adafruit_NeoPixel called strip
  Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRB + NEO_KHZ800);

    // IMPORTANT: To reduce NeoPixel burnout risk, add 1000 uF capacitor across
    // pixel power leads, add 300 - 500 Ohm resistor on first pixel's data input
    // and minimize distance between Arduino and first pixel.  Avoid connecting
    // on a live circuit...if you must, connect GND first.

  

  //~~~~~~~~~~~NeoPixel Functions~~~~~~~~~~~~~~~~
  //~~~~~~~~~~used in neopixeltask( )~~~~~~~~~~~~
  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  // Input a value 0 to 255 to get a color value.
  // The colours are a transition r - g - b - back to r.
  uint32_t Wheel(byte WheelPos) {
    WheelPos = 255 - WheelPos;
    if(WheelPos < 85) {
      return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
    }
    if(WheelPos < 170) {
      WheelPos -= 85;
      return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
    }
    WheelPos -= 170;
    return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
  }

  // Fill the dots one after the other with a color
  void colorWipe(uint32_t c, uint8_t wait) {
    for(uint16_t i=0; i<strip.numPixels(); i++) {
      strip.setPixelColor(i, c);
      strip.show();
      vTaskDelay(wait);
    }
  }

  void rainbow(uint8_t wait) {
    uint16_t i, j;

    for(j=0; j<256; j++) {
      for(i=0; i<strip.numPixels(); i++) {
        strip.setPixelColor(i, Wheel((i+j) & 255));
      }
      strip.show();
      vTaskDelay(wait);
    }
  }

  // Slightly different, this makes the rainbow equally distributed throughout
  void rainbowCycle(uint8_t wait) {
    uint16_t i, j;

    for(j=0; j<256*5; j++) { // 5 cycles of all colors on wheel
      for(i=0; i< strip.numPixels(); i++) {
        strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
      }
      strip.show();
      vTaskDelay(wait);
    }
  }

  //Theatre-style crawling lights.
  void theaterChase(uint32_t c, uint8_t wait) {
    for (int j=0; j<10; j++) {  //do 10 cycles of chasing
      for (int q=0; q < 3; q++) {
        for (uint16_t i=0; i < strip.numPixels(); i=i+3) {
          strip.setPixelColor(i+q, c);    //turn every third pixel on
        }
        strip.show();

        vTaskDelay(wait);

        for (uint16_t i=0; i < strip.numPixels(); i=i+3) {
          strip.setPixelColor(i+q, 0);        //turn every third pixel off
        }
      }
    }
  }

  //Theatre-style crawling lights with rainbow effect
  void theaterChaseRainbow(uint8_t wait) {
    for (int j=0; j < 256; j++) {     // cycle all 256 colors in the wheel
      for (int q=0; q < 3; q++) {
        for (uint16_t i=0; i < strip.numPixels(); i=i+3) {
          strip.setPixelColor(i+q, Wheel( (i+j) % 255));    //turn every third pixel on
        }
        strip.show();

        vTaskDelay(wait);

        for (uint16_t i=0; i < strip.numPixels(); i=i+3) {
          strip.setPixelColor(i+q, 0);        //turn every third pixel off
        }
      }
    }
  }

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~OLED FUNCTIONS - ADAFRUIT LIB~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~	

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define NUMFLAKES     10 // Number of snowflakes in the animation example

#define LOGO_HEIGHT   16
#define LOGO_WIDTH    16
static const unsigned char PROGMEM logo_bmp[] =
{ 0b00000000, 0b11000000,
  0b00000001, 0b11000000,
  0b00000001, 0b11000000,
  0b00000011, 0b11100000,
  0b11110011, 0b11100000,
  0b11111110, 0b11111000,
  0b01111110, 0b11111111,
  0b00110011, 0b10011111,
  0b00011111, 0b11111100,
  0b00001101, 0b01110000,
  0b00011011, 0b10100000,
  0b00111111, 0b11100000,
  0b00111111, 0b11110000,
  0b01111100, 0b11110000,
  0b01110000, 0b01110000,
  0b00000000, 0b00110000 };

  void testdrawline() {
  int16_t i;

  display.clearDisplay(); // Clear display buffer

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, 0, i, display.height()-1, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn line
    vTaskDelay(1);
  }
  for(i=0; i<display.height(); i+=4) {
    display.drawLine(0, 0, display.width()-1, i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  vTaskDelay(250);

  display.clearDisplay();

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(0, display.height()-1, display.width()-1, i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  vTaskDelay(250);

  display.clearDisplay();

  for(i=display.width()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, 0, i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  vTaskDelay(250);

  display.clearDisplay();

  for(i=0; i<display.height(); i+=4) {
    display.drawLine(display.width()-1, 0, 0, i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }
  for(i=0; i<display.width(); i+=4) {
    display.drawLine(display.width()-1, 0, i, display.height()-1, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000); // Pause for 2 seconds
}

void testdrawrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2; i+=2) {
    display.drawRect(i, i, display.width()-2*i, display.height()-2*i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testfillrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2; i+=3) {
    // The INVERSE color is used so rectangles alternate white/black
    display.fillRect(i, i, display.width()-i*2, display.height()-i*2, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn rectangle
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testdrawcircle(void) {
  display.clearDisplay();

  for(int16_t i=0; i<max(display.width(),display.height())/2; i+=2) {
    display.drawCircle(display.width()/2, display.height()/2, i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testfillcircle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=3) {
    // The INVERSE color is used so circles alternate white/black
    display.fillCircle(display.width() / 2, display.height() / 2, i, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn circle
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testdrawroundrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2-2; i+=2) {
    display.drawRoundRect(i, i, display.width()-2*i, display.height()-2*i,
      display.height()/4, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testfillroundrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2-2; i+=2) {
    // The INVERSE color is used so round-rects alternate white/black
    display.fillRoundRect(i, i, display.width()-2*i, display.height()-2*i,
      display.height()/4, SSD1306_INVERSE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testdrawtriangle(void) {
  display.clearDisplay();

  for(int16_t i=0; i<max(display.width(),display.height())/2; i+=5) {
    display.drawTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_WHITE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testfilltriangle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=5) {
    // The INVERSE color is used so triangles alternate white/black
    display.fillTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_INVERSE);
    display.display();
    vTaskDelay(1);
  }

  vTaskDelay(2000);
}

void testdrawchar(void) {
  display.clearDisplay();

  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0, 0);     // Start at top-left corner
  display.cp437(true);         // Use full 256 char 'Code Page 437' font

  // Not all the characters will fit on the display. This is normal.
  // Library will draw what it can and the rest will be clipped.
  for(int16_t i=0; i<256; i++) {
    if(i == '\n') display.write(' ');
    else          display.write(i);
  }

  display.display();
  vTaskDelay(2000);
}

void testdrawstyles(void) {
  display.clearDisplay();

  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("Hello, world!"));

  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.println(3.141592);

  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.print(F("0x")); display.println(0xDEADBEEF, HEX);

  display.display();
  vTaskDelay(2000);
}

void testscrolltext(void) {
  display.clearDisplay();

  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("scroll"));
  display.display();      // Show initial text
  vTaskDelay(100);

  // Scroll in various directions, pausing in-between:
  display.startscrollright(0x00, 0x0F);
  vTaskDelay(2000);
  display.stopscroll();
  vTaskDelay(1000);
  display.startscrollleft(0x00, 0x0F);
  vTaskDelay(2000);
  display.stopscroll();
  vTaskDelay(1000);
  display.startscrolldiagright(0x00, 0x07);
  vTaskDelay(2000);
  display.startscrolldiagleft(0x00, 0x07);
  vTaskDelay(2000);
  display.stopscroll();
  vTaskDelay(1000);
}

void testdrawbitmap(void) {
  display.clearDisplay();

  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.display();
  vTaskDelay(1000);
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~POT CONTROL TASK~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void controltaskx(void* arg)   //short test task
{  
  while(true)
  {
    vTaskDelay(1);
  
    float val = analogRead(myPedal->control[0].pin);
    myPedal->control[0].value =val;
    Serial.printf("CTRL-%d %s: %d\n",0,myPedal->control[0].name.c_str(),myPedal->control[0].value);
	}
}

void controltask(void* arg)
{ 
  int controlState[6];
  int stateCounter[6];
  //Serial.println("Starting controltask");

  while(true)
  {
    vTaskDelay(1);
    runningTicks++;
    
    for(int i=0;i<6;i++)  //service each of 6 possible pot controllers
    {
      
      ////////////////////////////////////////////////////////////////////////////////
      //POTENTIOMETER CONTROL MODE
      if(myPedal->control[i].mode == CM_POT)
      {
        //read the analog port
        float val = analogRead(myPedal->control[i].pin);
        if(myPedal->control[i].inverted)
        val = 4095-val;

          int increment = 4096/myPedal->control[i].levelCount;
          int position = val/increment;
          
          //normalize the unexpected
          if(position >= myPedal->control[i].levelCount)
            position = myPedal->control[i].levelCount -1;
          if(position < 0) position = 0;

          //add some hysteresis for stability
          int modulus = (int)val % increment;
          if(position > myPedal->control[i].value)
          {
            if((position-myPedal->control[i].value > 1)||(modulus > (increment>>2)))
            {
              myPedal->control[i].value = position;
              myPedal->onControlChange(i);
            }
          }
          else if(position < myPedal->control[i].value)
          {
            if(((myPedal->control[i].value - position) > 1)||(modulus < (increment-(increment>>2))))
            {
              myPedal->control[i].value = position;
              myPedal->onControlChange(i);
            }
          }
      }
      ////////////////////////////////////////////////////////////////////////////////
      //SELECTOR CONTROL MODE
      else if(myPedal->control[i].mode == CM_SELECTOR)
      {
        //read the analog port
        float val = analogRead(myPedal->control[i].pin);
        if(myPedal->control[i].inverted)
        val = 4095-val;

        //find the selector channel from val
        int channelwidth = 4096/myPedal->control[i].levelCount;
        int readchannel = val/channelwidth;

        //normalize the unexpected
        if(readchannel >= myPedal->control[i].levelCount)
          readchannel = myPedal->control[i].levelCount -1;
        if(readchannel < 0) readchannel = 0;
          
        if(readchannel != myPedal->control[i].value) //the value has changed
        {
          myPedal->control[i].value = readchannel;
          myPedal->onControlChange(i);
        }
            
      }
      ////////////////////////////////////////////////////////////////////////////////
      //TOGGLE PUSH BUTTON CONTROL MODE
      else if(myPedal->control[i].mode == CM_TOGGLE)
      {
        //read the analog port
        float val = analogRead(myPedal->control[i].pin);
        if(myPedal->control[i].inverted)
        val = 4095-val;

        switch(controlState[i])
        {
          case 0: //wait press
          {
            if(val < 2048)
            {
              controlState[i] = 1; //wait stable press
              stateCounter[i]=0;
            }
            break;
          }
          case 1: //wait stable press
          {
            if(val < 2048)
            {
              stateCounter[i]++;
              if(stateCounter[i] > 2)
              {
                if(myPedal->control[i].value==0)
                  myPedal->control[i].value=1;
                else myPedal->control[i].value=0;
                
                myPedal->onControlChange(i);
                controlState[i] = 2; //wait release
              }
            }
            else //not < 2048
            {
              controlState[i] = 0; //back to wait press
            }
            break;
          }
          case 2: //wait release
          {
            if(val > 2048)
            {
              controlState[i] = 3; //wait stable release
              stateCounter[i] = 0;
            }
          }
          case 3: //wait stable release
          {
            if(val > 2048)
            {
              stateCounter[i]++;
              if(stateCounter[i] > 2)
              {
                controlState[i] = 0; //back to wait press
              }
            }
            else controlState[i] = 2; //back to wait release
            break;
          }
        }
      }
      ////////////////////////////////////////////////////////////////////////////////
      //MOMENTARY PUSH BUTTON CONTROL MODE
      else if(myPedal->control[i].mode == CM_MOMENTARY)
      {
        //read the analog port
        float val = analogRead(myPedal->control[i].pin);
        if(myPedal->control[i].inverted)
        val = 4095-val;

          int tempval = 0;
          if(val < 2048)
            tempval = 1;
          if(myPedal->control[i].value == tempval)
            stateCounter[i]=0;
          else
          {
            stateCounter[i]++;
            if(stateCounter[i] > 2)
            {
              myPedal->control[i].value = tempval;
              myPedal->onControlChange(i);
            }
          }
      }
      
      else //mode = CM_DISABLED
      {
           //do nothing
      }
    }
  }
}  //End Control Task

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~BUTTON CONTROL TASK~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void buttontask(void* arg)
{  
  //state variables
  int bstate[6];
  int bstatecounter[6];
  
		//initialize pin mode
	  //do this in the main effects sketch
		//pinMode(pin, INPUT_PULLUP);
		
  for(int i=0;i<6;i++) 
  {
    bstate[i]=0;
    bstatecounter[i] = 0;
  }

  while(true)
  {
    vTaskDelay(1);

    for(int i=0;i<6;i++)  //service each of 6 possible switches
    {
      
      if(myPedal->button[i].mode == BM_TOGGLE)
      { 
        int temp = 0;
        if(myPedal->button[i].touch){
            if(touchRead(myPedal->button[i].pin) >= TOUCH_THRESHOLD)
                 { temp = 0; }
            else { temp = 1; }
        }
        else {
              temp = !digitalRead(myPedal->button[i].pin);
        }

        if(myPedal->button[i].inverted)
        temp = !temp;
        
        if(temp!= bstate[i])
        {
          bstatecounter[i]++;
          if(bstatecounter[i]>9) //debouncing
          {
            bstate[i] = temp;
            bstatecounter[i]=0;
            if(temp)
            {
              myPedal->onButtonPress(i);
              if(myPedal->button[i].value == 1)
                myPedal->button[i].value = 0;
              else
                myPedal->button[i].value = 1;
                
              myPedal->onButtonChange(i);
            }
            else
            {
              myPedal->onButtonRelease(i);
            }
          }
        }
        else bstatecounter[i]=0;
      }
      else if(myPedal->button[i].mode == BM_MOMENTARY)
      {
        int temp = 0;
        if(myPedal->button[i].touch){
            if(touchRead(myPedal->button[i].pin) >= TOUCH_THRESHOLD)
                 { temp = 0; }
            else { temp = 1; }
        }
        else {
              temp = !digitalRead(myPedal->button[i].pin);
        }
        if(myPedal->button[i].inverted)
        temp = !temp;
        
        if(temp != myPedal->button[i].value)
        {
          bstatecounter[i]++;
          if(bstatecounter[i]>9) //debouncing
          {
            bstatecounter[i]=0;
            if(temp==0)
            {
              myPedal->onButtonRelease(i);
              myPedal->button[i].value = 0;
              myPedal->onButtonChange(i);
            }
            else //temp=1
            {
             myPedal->onButtonPress(i);
              myPedal->button[i].value = 1;
              myPedal->onButtonChange(i);
            }
          }
        }
        else //temp = myPedal->button[i].value
        {
          bstatecounter[i]=0;
        }
      }

    }
  }
} //End Button task

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~FRAME COUNTER TASK~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void framecounter_task(void* arg)  //Only used for System Monitor
{
  while(true)
  {
    audiofps = processedframe;
    processedframe = 0;
    vTaskDelay(1000);
  }
  vTaskDelete(NULL);
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~NEOPIXEL LED TASK~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void neopixeltask(void* arg)
{   
  strip.setBrightness(BRIGHTNESS);
  strip.begin(); 
  strip.show(); // Initialize all pixels to 'off'

while(true)  //NeoPixel functions below defined at start of task.cpp
  {

    //Some example procedures showing how to display to the pixels:
    //colorWipe(strip.Color(255, 0, 0), 50); // Red
    //colorWipe(strip.Color(0, 255, 0), 50); // Green
    //colorWipe(strip.Color(0, 0, 255), 50); // Blue
    //colorWipe(strip.Color(0, 0, 0, 255), 50); // White RGBW

    int pixcolor = (int8_t) (myPedal->depth * 255); //control color

    // Send a theater pixel chase, "gain" controls red to blue
    theaterChase(strip.Color(pixcolor, 0, (255 - pixcolor)), 20); 
    //theaterChase(strip.Color(127, 0, 0), 50); // Red
    //theaterChase(strip.Color(0, 0, 127), 50); // Blue

    //rainbow(20);
    //rainbowCycle(20);
    //theaterChaseRainbow(50);
  }
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~OLED DISPLAY TASK~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

/**************************************************************************
 This is an example for our Monochrome OLEDs based on SSD1306 drivers
 The Yellow/Blue Display has a fixed upper banner of yellow 
 and main lower screen of blue pixels.

 Pick one up today in the adafruit shop!
 ------> http://www.adafruit.com/category/63_98

 This example is for a 128x64 pixel display using I2C to communicate
 3 pins are required to interface (two I2C and one reset).

 Adafruit invests time and resources providing this open
 source code, please support Adafruit and open-source
 hardware by purchasing products from Adafruit!

 Written by Limor Fried/Ladyada for Adafruit Industries,
 with contributions from the open source community.
 BSD license, check license.txt for more information
 All text above, and the splash screen below must be
 included in any redistribution.
 **************************************************************************/

void displaytask(void* arg)
{   
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

while(true)  //OLED Display functions below defined above in task.cpp
{

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  //display.display();
  //delay(2000); // Pause for 2 seconds

  // Clear the buffer
  //display.clearDisplay();

  // Draw a single pixel in white
  //display.drawPixel(10, 10, SSD1306_WHITE);

  // Show the display buffer on the screen. You MUST call display() after
  // drawing commands to make them visible on screen!
  //display.display();
  //vTaskDelay(2000);
  // display.display() is NOT necessary after every single drawing command,
  // unless that's what you want...rather, you can batch up a bunch of
  // drawing operations and then update the screen all at once by calling
  // display.display(). These examples demonstrate both approaches...

  //testdrawline();      // Draw many lines
  //testdrawrect();      // Draw rectangles (outlines)
  //testfillrect();      // Draw rectangles (filled)
  //testdrawcircle();    // Draw circles (outlines)
  //testfillcircle();    // Draw circles (filled)
  //testdrawroundrect(); // Draw rounded rectangles (outlines)
  //testfillroundrect(); // Draw rounded rectangles (filled)
  //testdrawtriangle();  // Draw triangles (outlines)
  //testfilltriangle();  // Draw triangles (filled)
  //testdrawchar();      // Draw characters of the default font
  //testdrawstyles();    // Draw 'stylized' characters
  //testscrolltext();    // Draw scrolling text
  //testdrawbitmap();    // Draw a small bitmap image

  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  
  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.println("Depth/Freq");  // will display in top yellow "banner" region
  display.print("   "); display.println(myPedal->depth);
  display.print("   "); display.println(myPedal->freq);
  display.print("   "); display.println(myPedal->pan);
  display.invertDisplay(true);

  display.display();

  vTaskDelay(100);
 
} // End of while(1)
} // End of task


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~DEBUG VARIABLES~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

static char debugStringPtr[50] = "None";
static float debugVars[]={0,0,0,0};

void setDebugStr(const char* str)
{
	//debugStringPtr = (char*) str;
	
}
void setDebugVars(float val1, float val2, float val3, float val4)
{
	debugVars[0]=val1;
	debugVars[1]=val2;
	debugVars[2]=val3;
	debugVars[3]=val4;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~MAIN BLACKSTOMP~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void taskSetup() 
{
  //validate the pot settings, correct for user mistakes in settings_main.h
  for(int i=0;i<6;i++)
  {
	  switch(myPedal->control[i].mode)
	  {
		  case CM_POT:
		  {
			  if(myPedal->control[i].levelCount > 256)
				myPedal->control[i].levelCount = 256;
			  if(myPedal->control[i].levelCount < 2)
				myPedal->control[i].levelCount = 2;
			  myPedal->control[i].min = 0;
			  myPedal->control[i].max = myPedal->control[i].levelCount -1;
			  break;
		  }
		  case CM_SELECTOR:
		  {
			  if(myPedal->control[i].levelCount > 12)
				myPedal->control[i].levelCount = 12;
			  if(myPedal->control[i].levelCount < 2)
				myPedal->control[i].levelCount = 2;
			  myPedal->control[i].min = 0;
			  myPedal->control[i].max = myPedal->control[i].levelCount -1;
			  break;
		  }
		  case CM_DISABLED:
		  {
			// do nothing
			break;
		  }
		  case CM_TOGGLE:
		  {
			// do nothing
			break;
		  }
		  case CM_MOMENTARY:
		  {
			// do nothing
			break;
		  }
	  }
  }
  
  
	//the main audio processing task is placed in the main loop() of main.cpp (core1) 

	//decoding button presses 
	xTaskCreatePinnedToCore(buttontask, "buttontask", 4096, NULL, AUDIO_PROCESS_PRIORITY, NULL,0);

	//decoding potentiometer and other analog sensors
	xTaskCreatePinnedToCore(controltask, "controltask", 4096, NULL, AUDIO_PROCESS_PRIORITY, NULL,0);

	//audio frame monitoring task used by systemMonitor
	xTaskCreatePinnedToCore(framecounter_task, "framecounter_task", 4096, NULL, AUDIO_PROCESS_PRIORITY, NULL,0);

  //Neo Pixel Task originally for LillyGo TAudio board with 19 LEDs
	//xTaskCreatePinnedToCore(neopixeltask, "NeoPixeltask", 4096, NULL, 5, NULL,0);

  //OLED Display Task for SSD1306 0.96" OLED Display Module using Adafruit Library
	xTaskCreatePinnedToCore(displaytask, "OLED_Displaytask", 4096, NULL, 5, NULL,0);


}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~SYSTEM MONITOR TASK~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void sysmon_task(void *arg)
{
	int* period = (int*)(arg);
	while(true)
	{
	  //System info
	  Serial.printf("\nSYSTEM INFO:\n");
	  Serial.printf("Internal Total heap %d, internal Free Heap %d\n",ESP.getHeapSize(),ESP.getFreeHeap());
	  Serial.printf("SPIRam Total heap %d, SPIRam Free Heap %d\n",ESP.getPsramSize(),ESP.getFreePsram());
	  Serial.printf("ChipRevision %d, Cpu Freq %d, SDK Version %s\n",ESP.getChipRevision(), ESP.getCpuFreqMHz(), ESP.getSdkVersion());
	  Serial.printf("Flash Size %d, Flash Speed %d\n",ESP.getFlashChipSize(), ESP.getFlashChipSpeed());
	  
	  //Blackstomp application info
	  Serial.printf("\nAPPLICATION INFO:\n");
	  Serial.printf("Pedal Name: %s\n",myPedal->name.c_str());
	  Serial.printf("Audio frame per second: %d fps\n",audiofps);
	  Serial.printf("CPU ticks per frame period: %d\n",availableticks);
	  Serial.printf("Used CPU ticks: %d\n",usedticks);
    Serial.printf("running ticks: %d\n",runningTicks);
	  Serial.printf("CPU Usage: %.2f %%\n", 100.0*((float)usedticks/(float)availableticks));
	  for(int i=0;i<6;i++)
	  {
		  if(myPedal->control[i].mode != 0)  // not CM_DISABLED
			Serial.printf("CTRL-%d %s: %d\n",i,myPedal->control[i].name.c_str(),myPedal->control[i].value);
	  }
	  for(int i=0;i<6;i++)
	  {
		  if(myPedal->button[i].mode != 0)  // not CM_DISABLED
			Serial.printf("BUTTON-%d %s: %d\n",i,myPedal->button[i].name.c_str(),myPedal->button[i].value);
	  }
	  char debugstring[51];
	  strncpy(debugstring,debugStringPtr,50);
	  Serial.printf("Debug String: %s\n", debugstring);
	  Serial.printf("Debug Variables: %g, %g, %g, %g\n", debugVars[0], debugVars[1], debugVars[2], debugVars[3]);
	  vTaskDelay(period[0]);
	}
	vTaskDelete(NULL);
}

int _updatePeriod;
void runSystemMonitor(int baudRate, int updatePeriod)
{
	Serial.begin(baudRate);
	_updatePeriod = updatePeriod;
	//run the performance monitoring task at 0 (idle) priority
	xTaskCreatePinnedToCore(sysmon_task, "sysmon_task", 4096, &_updatePeriod, 0, NULL,0);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~SCOPE TASK~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

enum {scp_disabled, scp_startWaitTrigger, scp_waitTrigger, scp_probing, scp_waitdisplay};
static int scpState = scp_disabled;

static int scSampleCount;
static int scSampleLength;
static int scTriggerChannel;
static float scPrevSample;
static float scTriggerLevel;
static bool scRisingTrigger;
static float** scData = NULL;

void scope_task(void *arg)
{
	scSampleCount = 0;
	scpState = scp_startWaitTrigger;
	while(true)
	{
		if(scpState==scp_waitdisplay)
		{
			//send the data to serial display
			for(int j=0;j<scSampleLength;j++)
			{
					Serial.printf("ch-%d:%f, ch-%d:%f\n", 0,scData[0][j],1,scData[1][j]);
			}
			Serial.flush();
			vTaskDelay(1);
			scpState = scp_startWaitTrigger;
		}	
		vTaskDelay(100);
	}
	vTaskDelete(NULL);
}

void runScope(int baudRate, int sampleLength, int triggerChannel, float triggerLevel, bool risingTrigger)
{
	Serial.begin(baudRate);
	scSampleLength = sampleLength;
	scTriggerLevel = triggerLevel;
	scRisingTrigger = risingTrigger;
	scTriggerChannel = triggerChannel;
	
	scData = new float*[2];
	for(int i=0;i<2;i++)
		scData[i]=(float*)ps_malloc((scSampleLength +1) * sizeof(float));
	for(int i=0;i<scSampleLength;i++)
	{
		scData[0][i] = 0;
		scData[1][i] = 0;
	}
	xTaskCreatePinnedToCore(scope_task, "scope_task", 4096, NULL, AUDIO_PROCESS_PRIORITY-1, NULL,0);
}

void scopeProbe(float sample, int channel)
{
	switch(scpState)
	{
		case scp_probing:
		{
			if(scSampleCount < scSampleLength)
			scData[channel][scSampleCount] = sample;
			if(channel == scTriggerChannel)
			{
				scSampleCount++;
				if(scSampleCount>= scSampleLength)
				{
					scpState = scp_waitdisplay;
				}
			}
			break;
		}
		case scp_startWaitTrigger:
		{
			if(channel == scTriggerChannel)
			{
				scPrevSample = sample;
				scpState = scp_waitTrigger;
			}
			break;
		}
		case scp_waitTrigger:
		{
			if(channel == scTriggerChannel)
			{
				if(scRisingTrigger)
				{
					if((sample > scTriggerLevel)&&(scPrevSample <= scTriggerLevel))
					{
						scSampleCount = 0;
						scpState = scp_probing;
					}
				}
				else 
				{
					if((sample < scTriggerLevel)&&(scPrevSample >= scTriggerLevel))
					{
						scSampleCount = 0;
						scpState = scp_probing;
					}
				}
				scPrevSample = sample;

			}
			break;
		}
	}
}

