#include <Arduino.h>
#include <driver/i2s.h>
#include <driver/adc.h>
#include <soc/syscon_reg.h>
#include <TFT_eSPI.h>
#include <SPI.h>
#include "esp_adc_cal.h"


//#define DEBUG_SERIAL
//#define DEBUG_BUFF
#define DELAY 1000

// Width and height of sprite
#define WIDTH  240
#define HEIGHT 320

#define ADC_CHANNEL   ADC1_CHANNEL_6  // GPIO33
#define NUM_SAMPLES   1000            // number of samples
#define I2S_NUM         (0)
#define BUFF_SIZE 50000
#define B_MULT BUFF_SIZE/NUM_SAMPLES
#define BUTTON_Ok        32
#define BUTTON_Plus        25
#define BUTTON_Minus        33
#define BUTTON_Back        26

#define TFT_MOSI 23 // In some display driver board, it might be written as "SDA" and so on.
#define TFT_SCLK 18
#define TFT_CS   15  // Chip select control pin
#define TFT_DC   2  // Data Command control pin
#define TFT_RST  4  // Reset pin (could connect to Arduino RESET pin)
#define TFT_BL   21  // LED back-light

TFT_eSPI    tft = TFT_eSPI();         // Declare object "tft"

TFT_eSprite spr = TFT_eSprite(&tft);  // Declare Sprite object "spr" with pointer to "tft" object



esp_adc_cal_characteristics_t adc_chars;

TaskHandle_t task_menu;
TaskHandle_t task_adc;

float v_div = 825;
float s_div = 10;
float offset = 0;
float toffset = 0;
uint8_t current_filter = 1;

//options handler
enum Option {
  None,
  Autoscale,
  Vdiv,
  Sdiv,
  Offset,
  TOffset,
  Filter,
  Stop,
  Mode,
  Single,
  Clear,
  Reset,
  Probe,
  UpdateF,
  Cursor1,
  Cursor2
};

int8_t volts_index = 0;

int8_t tscale_index = 0;

uint8_t opt = None;

bool menu = false;
bool info = true;
bool set_value  = false;

float RATE = 1000; //in ksps --> 1000 = 1Msps

bool auto_scale = false;

bool full_pix = true;

bool stop = false;

bool stop_change = false;

uint16_t i2s_buff[BUFF_SIZE];

bool single_trigger = false;
bool data_trigger = false;

bool updating_screen = false;
bool new_data = false;
bool menu_action = false;
uint8_t digital_wave_option = 0; //0-auto | 1-analog | 2-digital data (SERIAL/SPI/I2C/etc)
int btnok,btnpl,btnmn,btnbk;

class low_pass {
  public:

    low_pass(int factor) {
      _factor = factor;
    }

    float filter(float reading) {
      _value = _value * (_factor) + reading * (1.0 - _factor);
      return _value;
    }

    float _value = 0;
    float _factor = 0.99;

};


class mean_filter {
  public:

    mean_filter(int values) {
      _values = values;
    }

    void init(float value){
      for(int i=0;i<_values;i++){
        _data[i] = value;
      }
    }
    float filter(float reading) {
      float temp = 0;
      _data[_values - 1] = reading;
      for (int i = 0; i < _values - 1; i++) {
        temp += _data[i];
        _data[i] = _data[i + 1];
      }
      temp += reading;
      return temp / float(_values);
    }
    
    int _values = 5;
    float _data[100] = {0};
};

void configure_i2s(int rate) {
  /*keep in mind:
     dma_buf_len * dma_buf_count * bits_per_sample/8 > 4096
  */
  i2s_config_t i2s_config =
  {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_ADC_BUILT_IN),  // I2S receive mode with ADC
    .sample_rate = rate,                                                          // sample rate
    .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,                                 // 16 bit I2S
    .channel_format = I2S_CHANNEL_FMT_ALL_LEFT,                                   // only the left channel
    .communication_format = (i2s_comm_format_t)(I2S_COMM_FORMAT_I2S | I2S_COMM_FORMAT_I2S_MSB),   // I2S format
    .intr_alloc_flags = 1,                                                        // none
    .dma_buf_count = 2,                                                           // number of DMA buffers
    .dma_buf_len = NUM_SAMPLES,                                                   // number of samples
    .use_apll = 0,                                                                // no Audio PLL
  };
  adc1_config_channel_atten(ADC_CHANNEL, ADC_ATTEN_DB_11);
  adc1_config_width(ADC_WIDTH_BIT_12);
  i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL);

  i2s_set_adc_mode(ADC_UNIT_1, ADC_CHANNEL);
  SET_PERI_REG_MASK(SYSCON_SARADC_CTRL2_REG, SYSCON_SARADC_SAR1_INV);
  i2s_adc_enable(I2S_NUM_0);
}

void ADC_Sampling(uint16_t *i2s_buff){

  size_t bytes_read; for (int i = 0; i < B_MULT; i++) {

    i2s_read(I2S_NUM_0, (void*)&i2s_buff[i * NUM_SAMPLES], NUM_SAMPLES * sizeof(uint16_t), &bytes_read, portMAX_DELAY);

    for(size_t ix = 0; ix < bytes_read/2; ix++) i2s_buff[(i * NUM_SAMPLES) + ix] &= 0x0FFF; // 16bit to 12bit conversion 

  }

}

void set_sample_rate(uint32_t rate) {
  i2s_driver_uninstall(I2S_NUM_0);
  configure_i2s(rate);
}
int voltage_division[6] = { //screen has 4 divisions, 31 pixels each (125 pixels of height)
  1000, //fullscreen 3.3V peak-peak
  500,
  375,
  180,
  100,
  50
};


/*each sample represents 1us (1Msps),
   thus, the time division is the number
   of samples per screen division
*/
float time_division[10] = { //screen has 4 divisions, 60 pixel each (240 pixel of width)
  10,
  25,
  50,
  100,
  250,
  500,
  1000,
  2500,
  5000,
  10000
};
//,   //1Mhz    35ms of data (of 50ms possible)
//  10000,  //100khz  70ms/500ms
//  25000,  //100khz  175ms/500ms of data
//  50000,  //100khz  350ms/500ms of data
//  100000  //50khz   700ms/1000ms of data
//};


void hide_menu() {
  menu = false;
}

void hide_all() {
  menu = false;
  info = false;
}

void show_menu() {
  menu = true;
}

void button() {
  if ( btnok == 1 || btnbk == 1 || btnpl == 1 || btnmn == 1)
  {
    menu_action = true;
  }
  if (menu == true)
  {
    if (set_value) {
      switch (opt) {
        case Vdiv:
          if (btnpl == 1) {
            volts_index++;
            if (volts_index >= sizeof(voltage_division) / sizeof(*voltage_division)) {
              volts_index = 0;
            }
            btnpl = 0;
          }
          else if (btnmn == 1) {
            volts_index--;
            if (volts_index < 0) {
              volts_index = sizeof(voltage_division) / sizeof(*voltage_division) - 1;
            }
            btnmn = 0;
          }

          v_div = voltage_division[volts_index];
          break;

        case Sdiv:
          if (btnmn == 1) {
            tscale_index++;
            if (tscale_index >= sizeof(time_division) / sizeof(*time_division)) {
              tscale_index = 0;
            }
            btnmn = 0;
          }
          else if (btnpl == 1) {
            tscale_index--;
            if (tscale_index < 0) {
              tscale_index = sizeof(time_division) / sizeof(*time_division) - 1;
            }
            btnpl = 0;
          }

          s_div = time_division[tscale_index];
          break;

        case Offset:
          if (btnmn == 1) {
            offset += 0.1 * (v_div * 4) / 3300;
            btnmn = 0;
          }
          else if (btnpl == 1) {
            offset -= 0.1 * (v_div * 4) / 3300;
            btnpl = 0;
          }

          if (offset > 3.3)
            offset = 3.3;
          if (offset < -3.3)
            offset = -3.3;

          break;

        case TOffset:
          if (btnpl == 1)
          {
            toffset += 0.1 * s_div;
            btnpl = 0;
          }
          else if (btnmn == 1)
          {
            toffset -= 0.1 * s_div;
            btnmn = 0;
          }

          break;

        default:
          break;

      }
      if (btnbk == 1)
      {
        set_value = 0;
        btnbk = 0;
      }
    }
    else
    {
      if (btnpl == 1)
      {
        opt++;
        if (opt > Single)
        {
          opt = 1;
        }
        Serial.print("option : ");
        Serial.println(opt);
        btnpl = 0;
      }
      if (btnmn == 1)
      {
        opt--;
        if (opt < 1)
        {
          opt = 9;
        }
        Serial.print("option : ");
        Serial.println(opt);
        btnmn = 0;
      }
      if (btnbk == 1)
      {
        hide_menu();
        btnbk = 0;
      }
      if (btnok == 1) {
        switch (opt) {
          case Autoscale:
            auto_scale = !auto_scale;
            break;

          case Vdiv:
            set_value = true;
            break;

          case Sdiv:
            set_value = true;
            break;

          case Offset:
            set_value = true;
            break;

          case Stop:
            stop = !stop;
            //Serial.print("Stop : ");
            //Serial.println(stop);
            set_value = false;
            break;

          case TOffset:
            set_value = true;
            //set_value = false;
            break;

          case Single:
            single_trigger = true;
            set_value = false;
            break;

          case Reset:
            offset = 0;
            v_div = 550;
            s_div = 10;
            tscale_index = 0;
            volts_index = 0;
            break;

          case Probe:
            break;

          case Mode:
            digital_wave_option++;
            if (digital_wave_option > 2)
              digital_wave_option = 0;
            break;

          case Filter:
            current_filter++;
            if (current_filter > 3)
              current_filter = 0;
            break;

          default:
            break;

        }

        btnok = 0;
      }
    }
  }
  else
  {
    if (btnok == 1)
    {
      opt = 1;
      show_menu();
      btnok = 0;
    }
    if (btnbk == 1)
    {
      if (info == true)
      {
        hide_all();
      }
      else
      {
        info = true;
      }
      btnbk = 0;
    }
    if (btnpl == 1) {
      volts_index++;
      if (volts_index >= sizeof(voltage_division) / sizeof(*voltage_division)) {
        volts_index = 0;
      }
      btnpl = 0;
      v_div = voltage_division[volts_index];
    }
    if (btnmn == 1) {
      tscale_index++;
      if (tscale_index >= sizeof(time_division) / sizeof(*time_division)) {
        tscale_index = 0;
      }
      btnmn = 0;
      s_div = time_division[tscale_index];
    }
  }

}


String strings_vdiv() {
  return "";
}

String strings_sdiv() {
  return "";
}

String strings_offset() {
  return "";
}

String strings_toffset() {
  return "";
}

String strings_freq() {
  return "";
}

String strings_peak() {
  return "";
}

String strings_vmax() {
  return "";
}

String strings_vmin() {
  return "";
}

String strings_filter() {
  return "";
}
void menu_handler() {
  button();
}
void characterize_adc() {
  esp_adc_cal_characterize(
    (adc_unit_t)ADC_UNIT_1,
    (adc_atten_t)ADC_CHANNEL,
    (adc_bits_width_t)ADC_WIDTH_BIT_12,
    1100,
    &adc_chars);
}

void peak_mean(uint16_t *i2s_buffer, uint32_t len, float * max_value, float * min_value, float *pt_mean) {
  max_value[0] = i2s_buffer[0];
  min_value[0] = i2s_buffer[0];
  mean_filter filter(5);
  filter.init(i2s_buffer[0]);

  float mean = 0;
  for (uint32_t i = 1; i < len; i++) {

    float value = filter.filter((float)i2s_buffer[i]);
    if (value > max_value[0])
      max_value[0] = value;
    if (value < min_value[0])
      min_value[0] = value;

    mean += i2s_buffer[i];
  }
  mean /= float(BUFF_SIZE);
  mean = to_voltage(mean);
  pt_mean[0] = mean;
}


//true if digital/ false if analog
bool digital_analog(uint16_t *i2s_buffer, uint32_t max_v, uint32_t min_v) {
  uint32_t upper_threshold = max_v - 0.05 * (max_v - min_v);
  uint32_t lower_threshold = min_v + 0.05 * (max_v - min_v);
  uint32_t digital_data = 0;
  uint32_t analog_data = 0;
  for (uint32_t i = 0; i < BUFF_SIZE; i++) {
    if (i2s_buffer[i] > lower_threshold) {
      if (i2s_buffer[i] > upper_threshold) {
        //HIGH DIGITAL
        digital_data++;
      }
      else {
        //ANALOG/TRANSITION
        analog_data++;
      }
    }
    else {
      //LOW DIGITAL
      digital_data++;
    }
  }

  //more than 50% of data is analog
  if (analog_data < digital_data)
    return true;

  return false;
}

void trigger_freq_analog(uint16_t *i2s_buffer,
                         float sample_rate,
                         float mean,
                         uint32_t max_v,
                         uint32_t min_v,
                         float *pt_freq,
                         float *pt_period,
                         uint32_t *pt_trigger0,
                         uint32_t *pt_trigger1) {
  float freq = 0;
  float period = 0;
  bool signal_side = false;
  uint32_t trigger_count = 0;
  uint32_t trigger_num = 10;
  uint32_t trigger_temp[trigger_num] = {0};
  uint32_t trigger_index = 0;

  //get initial signal relative to the mean
  if (to_voltage(i2s_buffer[0]) > mean) {
    signal_side = true;
  }


  //waveform repetitions calculation + get triggers time
  uint32_t wave_center = (max_v + min_v) / 2;
  for (uint32_t i = 1 ; i < BUFF_SIZE; i++) {
    if (signal_side && i2s_buffer[i] < wave_center - (wave_center - min_v) * 0.2) {
      signal_side = false;
    }
    else if (!signal_side && i2s_buffer[i] > wave_center + (max_v - wave_center) * 0.2) {
      freq++;
      if (trigger_count < trigger_num) {
        trigger_temp[trigger_count] = i;
        trigger_count++;
      }
      signal_side = true;
    }
  }

  //frequency calculation
  if (trigger_count < 2) {
    trigger_temp[0] = 0;
    trigger_index = 0;
    freq = 0;
    period = 0;
  }
  else {

    //simple frequency calculation fair enough for frequencies over 2khz (20hz resolution)
    freq = freq * 1000 / 50;
    period = (float)(sample_rate * 1000.0) / freq; //us

    //from 2000 to 80 hz -> uses mean of the periods for precision
    if (freq < 2000 && freq > 80) {
      period = 0;
      for (uint32_t i = 1; i < trigger_count; i++) {
        period += trigger_temp[i] - trigger_temp[i - 1];
      }
      period /= (trigger_count - 1);
      freq = sample_rate * 1000 / period;
    }

    //under 80hz, single period for frequency calculation
    else if (trigger_count > 1 && freq <= 80) {
      period = trigger_temp[1] - trigger_temp[0];
      freq = sample_rate * 1000 / period;
    }
  }

  //setting triggers offset and getting second trigger for debug cursor on drawn_channel1
  /*
     The trigger function uses a rise porcentage (5%) obove the mean, thus,
     the real waveform starting point is some datapoints back.
     The resulting trigger gets a negative offset of 5% of the calculated period
  */
  uint32_t trigger2 = 0;
  if (trigger_temp[0] - period * 0.05 > 0 && trigger_count > 1) {
    trigger_index = trigger_temp[0] - period * 0.05;
    trigger2 = trigger_temp[1] - period * 0.05;
  }
  else if (trigger_count > 2) {
    trigger_index = trigger_temp[1] - period * 0.05;
    if (trigger_count > 2)
      trigger2 = trigger_temp[2] - period * 0.05;
  }


  pt_trigger0[0] = trigger_index;
  pt_trigger1[0] = trigger2;
  pt_freq[0] = freq;
  pt_period[0] = period;

}


void trigger_freq_digital(uint16_t *i2s_buffer,
                          float sample_rate,
                          float mean,
                          uint32_t max_v,
                          uint32_t min_v,
                          float *pt_freq,
                          float *pt_period,
                          uint32_t *pt_trigger0) {

  float freq = 0;
  float period = 0;
  bool signal_side = false;
  uint32_t trigger_count = 0;
  uint32_t trigger_num = 10;
  uint32_t trigger_temp[trigger_num] = {0};
  uint32_t trigger_index = 0;

  //get initial signal relative to the mean
  if (to_voltage(i2s_buffer[0]) > mean) {
    signal_side = true;
  }

  //waveform repetitions calculation + get triggers time
  uint32_t wave_center = (max_v + min_v) / 2;
  bool normal_high = (mean > to_voltage(wave_center)) ? true : false;
  if (max_v - min_v > 4095 * (0.4 / 3.3)) {
    for (uint32_t i = 1 ; i < BUFF_SIZE; i++) {
      if (signal_side && i2s_buffer[i] < wave_center - (wave_center - min_v) * 0.2) {

        //signal was high, fell -> trigger if normal high
        if (trigger_count < trigger_num && normal_high) {
          trigger_temp[trigger_count] = i;
          trigger_count++;
        }

        signal_side = false;

      }
      else if (!signal_side && i2s_buffer[i] > wave_center + (max_v - wave_center) * 0.2) {
        freq++;

        //signal was low, rose -> trigger if normal low
        if (trigger_count < trigger_num && !normal_high) {
          trigger_temp[trigger_count] = i;
          trigger_count++;
        }

        signal_side = true;
      }
    }

    freq = freq * 1000 / 50;
    period = (float)(sample_rate * 1000.0) / freq; //us

    if (trigger_count > 1) {
      //from 2000 to 80 hz -> uses mean of the periods for precision
      if (freq < 2000 && freq > 80) {
        period = 0;
        for (uint32_t i = 1; i < trigger_count; i++) {
          period += trigger_temp[i] - trigger_temp[i - 1];
        }
        period /= (trigger_count - 1);
        freq = sample_rate * 1000 / period;
      }

      //under 80hz, single period for frequency calculation
      else if (trigger_count > 1 && freq <= 80) {
        period = trigger_temp[1] - trigger_temp[0];
        freq = sample_rate * 1000 / period;
      }
    }
    
    trigger_index = trigger_temp[0];

    if (trigger_index > 10)
      trigger_index -= 10;
    else
      trigger_index = 0;
  }




  pt_trigger0[0] = trigger_index;
  pt_freq[0] = freq;
  pt_period[0] = period;

}


void IRAM_ATTR btok()
{
  btnok = 1;
}
void IRAM_ATTR btplus()
{
  btnpl = 1;
}
void IRAM_ATTR btminus()
{
  btnmn = 1;
}
void IRAM_ATTR btback()
{
  btnbk = 1;
}
void setup() {
  Serial.begin(115200);

  configure_i2s(1000000);

  setup_screen();

  pinMode(BUTTON_Ok , INPUT);
  pinMode(BUTTON_Plus , INPUT);
  pinMode(BUTTON_Minus , INPUT);
  pinMode(BUTTON_Back , INPUT);
  attachInterrupt(BUTTON_Ok, btok, RISING);
  attachInterrupt(BUTTON_Plus, btplus, RISING);
  attachInterrupt(BUTTON_Minus, btminus, RISING);
  attachInterrupt(BUTTON_Back, btback, RISING);

  characterize_adc();
#ifdef DEBUG_BUF
  debug_buffer();
#endif

  xTaskCreatePinnedToCore(
    core0_task,
    "menu_handle",
    10000,  /* Stack size in words */
    NULL,  /* Task input parameter */
    0,  /* Priority of the task */
    &task_menu,  /* Task handle. */
    0); /* Core where the task should run */

  xTaskCreatePinnedToCore(
    core1_task,
    "adc_handle",
    10000,  /* Stack size in words */
    NULL,  /* Task input parameter */
    3,  /* Priority of the task */
    &task_adc,  /* Task handle. */
    1); /* Core where the task should run */
}


void core0_task( void * pvParameters ) {

  (void) pvParameters;

  for (;;) {
    menu_handler();

    if (new_data || menu_action) {
      new_data = false;
      menu_action = false;

      updating_screen = true;
      update_screen(i2s_buff, RATE);
      updating_screen = false;
      vTaskDelay(pdMS_TO_TICKS(10));
      Serial.println("CORE0");
    }

    vTaskDelay(pdMS_TO_TICKS(10));
  }

}

void core1_task( void * pvParameters ) {

  (void) pvParameters;

  for (;;) {
    if (!single_trigger) {
      while (updating_screen) {
        vTaskDelay(pdMS_TO_TICKS(1));
      }
      if (!stop) {
        if (stop_change) {
          i2s_adc_enable(I2S_NUM_0);
          stop_change = false;
        }
        ADC_Sampling(i2s_buff);
        new_data = true;
      }
      else {
        if (!stop_change) {
          i2s_adc_disable(I2S_NUM_0);
          i2s_zero_dma_buffer(I2S_NUM_0);
          stop_change = true;
        }
      }
      Serial.println("CORE1");
      vTaskDelay(pdMS_TO_TICKS(300));
    }
    else {
      float old_mean = 0;
      while (single_trigger) {
        stop = true;
        ADC_Sampling(i2s_buff);
        float mean = 0;
        float max_v, min_v;
        peak_mean(i2s_buff, BUFF_SIZE, &max_v, &min_v, &mean);

        //signal captured (pp > 0.4V || changing mean > 0.2V) -> DATA ANALYSIS
        if ((old_mean != 0 && fabs(mean - old_mean) > 0.2) || to_voltage(max_v) - to_voltage(min_v) > 0.05) {
          float freq = 0;
          float period = 0;
          uint32_t trigger0 = 0;
          uint32_t trigger1 = 0;

          //if analog mode OR auto mode and wave recognized as analog
          bool digital_data = !false;
          if (digital_wave_option == 1) {
            trigger_freq_analog(i2s_buff, RATE, mean, max_v, min_v, &freq, &period, &trigger0, &trigger1);
          }
          else if (digital_wave_option == 0) {
            digital_data = digital_analog(i2s_buff, max_v, min_v);
            if (!digital_data) {
              trigger_freq_analog(i2s_buff, RATE, mean, max_v, min_v, &freq, &period, &trigger0, &trigger1);
            }
            else {
              trigger_freq_digital(i2s_buff, RATE, mean, max_v, min_v, &freq, &period, &trigger0);
            }
          }
          else {
            trigger_freq_digital(i2s_buff, RATE, mean, max_v, min_v, &freq, &period, &trigger0);
          }

          single_trigger = false;
          new_data = true;
          Serial.println("Single GOT");
          //return to normal execution in stop mode
        }

        vTaskDelay(pdMS_TO_TICKS(1));   //time for the other task to start (low priorit)

      }
      vTaskDelay(pdMS_TO_TICKS(300));
    }
  }
}

void loop() {}