#include <WiFi.h> // This is the most basic WiFi implimentation
#include "esp32/ulp.h"
#include "driver/adc.h"         
#include <esp_bt.h>             
#include <esp_wifi.h>           
#include "soc/rtc_cntl_reg.h"
#include "driver/rtc_io.h"
#define uS_TO_S_FACTOR 1000000  
#define TIME_TO_SLEEP  300

void power_off(){
  setCpuFrequencyMhz(80); //Save power by stepping down CPU freq. NOTE: Test this, as it can affect some sensor reading
  btStop();               // Power down BT for best power saving
  esp_wifi_stop();        // Power down the Wifi radios for best power saving
  esp_deep_sleep_start(); 
  }
  

   const ulp_insn_t program[] = {
      I_DELAY(32000),              // Wait until ESP32 goes to deep sleep
      M_LABEL(1),                  // LABEL 1
        I_MOVI(R0, 0),             // Set reg. R0 to initial 0
        I_MOVI(R2, 0),             // Set reg. R2 to initial 0
      M_LABEL(2),                  // LABEL 2
        I_ADDI(R0, R0, 1),         // Increment cycle counter (reg. R0)
        I_ADC(R1, 0, 0),           // Read ADC value to reg. R1
        I_ADDR(R2, R2, R1),        // Add ADC value from reg R1 to reg. R2
      M_BL(2, 4),                  // If cycle counter is less than 4, go to LABEL 2
      I_RSHI(R0, R2, 2),           // Divide accumulated ADC value in reg. R2 by 4 and save it to reg. R0
      M_BGE(3, 1122), // If average ADC value from reg. R0 is higher or equal than high_adc_treshold, go to LABEL 3
      M_BL(3, 3344),   // If average ADC value from reg. R0 is lower than low_adc_treshold, go to LABEL 3
      M_BX(1),                     // Go to LABEL 1
      M_LABEL(3),                  // LABEL 3
      I_WAKE(),                    // Wake up ESP32
      I_END(),                     // Stop ULP program timer
      I_HALT()                     // Halt the coprocessor
   };



void ulp_adc_wake_up(unsigned int low_adc_treshold, unsigned int high_adc_treshold)
{
   adc1_config_channel_atten(ADC1_CHANNEL_4, ADC_ATTEN_DB_11);
   adc1_config_width(ADC_WIDTH_BIT_10);
   adc1_ulp_enable();

   rtc_gpio_init(GPIO_NUM_32);

   const ulp_insn_t program[] = {
      I_DELAY(32000),              // Wait until ESP32 goes to deep sleep
      M_LABEL(1),                  // LABEL 1
        I_MOVI(R0, 0),             // Set reg. R0 to initial 0
        I_MOVI(R2, 0),             // Set reg. R2 to initial 0
      M_LABEL(2),                  // LABEL 2
        I_ADDI(R0, R0, 1),         // Increment cycle counter (reg. R0)
        I_ADC(R1, 0, 0),           // Read ADC value to reg. R1
        I_ADDR(R2, R2, R1),        // Add ADC value from reg R1 to reg. R2
      M_BL(2, 4),                  // If cycle counter is less than 4, go to LABEL 2
      I_RSHI(R0, R2, 2),           // Divide accumulated ADC value in reg. R2 by 4 and save it to reg. R0
      M_BGE(3, high_adc_treshold), // If average ADC value from reg. R0 is higher or equal than high_adc_treshold, go to LABEL 3
      M_BL(3, low_adc_treshold),   // If average ADC value from reg. R0 is lower than low_adc_treshold, go to LABEL 3
      M_BX(1),                     // Go to LABEL 1
      M_LABEL(3),                  // LABEL 3
      I_WAKE(),                    // Wake up ESP32
      I_END(),                     // Stop ULP program timer
      I_HALT()                     // Halt the coprocessor
   };

   size_t size = sizeof(program)/sizeof(ulp_insn_t);
   ulp_process_macros_and_load(0, program, &size);

   ulp_run(0);
   esp_sleep_enable_ulp_wakeup();
   btStart();
   esp_wifi_start();
}


void setup() {

    
  Serial.begin(115200);
  //adc_power_on();    
  //power_off();
 
 
}

void loop()
{
  //ulp_adc_wake_up(0, 3500);
  Serial.printf("ulp size %d\n", sizeof(program));
}