#include <Arduino.h>

#include "ulp.h"
#include "driver/rtc_io.h"
#include "UlpDebug.h"

// Slow memory variable assignment
enum {
  SLOW_BLINK_STATE,     // Blink status

  SLOW_PROG_ADDR        // Program start address
};

void ULP_BLINK(uint32_t us) {
  // Set ULP activation interval
  ulp_set_wakeup_period(0, us);

  // Slow memory initialization
  memset(RTC_SLOW_MEM, 0, 8192);

  // Blink status initialization
  RTC_SLOW_MEM[SLOW_BLINK_STATE] = 0;

  // PIN to blink (specify by +14)
  // const int pin_blink_bit = RTCIO_GPIO26_CHANNEL + 14;
  // const gpio_num_t pin_blink = GPIO_NUM_26


  const int pin_blink_bit = RTCIO_GPIO4_CHANNEL + 14;
  const gpio_num_t pin_blink = GPIO_NUM_4;

  // GPIO26 initialization (set to output and initial value is 0)
  rtc_gpio_init(pin_blink);
  rtc_gpio_set_direction(pin_blink, RTC_GPIO_MODE_OUTPUT_ONLY);
  rtc_gpio_set_level(pin_blink, 1);

    enum lable_id {
    LBL_ON=10,
    LBL_OFF,
    LBL_DELAY
  };

  // ULP Program
  const ulp_insn_t  ulp_prog[] = {

    // I_WR_REG(RTC_GPIO_OUT_REG, pin_blink_bit, pin_blink_bit, 0),// pin_blink_bit = 0
    // I_END(),	
    // I_HALT(),

    M_LABEL( LBL_ON ),    // ON
    I_WR_REG(RTC_GPIO_OUT_REG, pin_blink_bit, pin_blink_bit, 1), // pin_blink_bit = 1
    I_MOVI( R1, 200 ),    // 200 mS
    M_MOVL( R2, LBL_OFF ),    // Label addr => R2
    M_BX( LBL_DELAY ),

    M_LABEL( LBL_OFF ),
    I_WR_REG(RTC_GPIO_OUT_REG, pin_blink_bit, pin_blink_bit, 0),// pin_blink_bit = 0
    I_MOVI( R1, 800 ),    // 800 mS
    M_MOVL( R2, LBL_ON ),
    M_BX( LBL_DELAY ),
   
    M_LABEL( LBL_DELAY ),
    I_DELAY( 8000 ),
    I_SUBI( R1, R1, 1 ),
    I_BXZR( R2 ),
    M_BX( LBL_DELAY ),

    I_MOVI( R0, LBL_ON ),
    M_MOVL( R0, LBL_ON ),    // Label addr => R2
    I_MOVI( R0, LBL_OFF ),
    M_MOVL( R0, LBL_OFF ),    // Label addr => R2
    I_MOVI( R0, LBL_DELAY ),
    M_MOVL( R0, LBL_DELAY ),    // Label addr => R2


    I_HALT()                                // Stop the program
  };

  // Run the program shifted backward by the number of variables
  size_t size = sizeof(ulp_prog) / sizeof(ulp_insn_t);
  ulp_process_macros_and_load(SLOW_PROG_ADDR, ulp_prog, &size);
  ulp_run(SLOW_PROG_ADDR);
}

void setup() {
  // For debug output
  Serial.begin(115200);

  // Execute ULP program at 300ms intervals
  // ULP_BLINK(300000);
}

void loop() {
  // For debug output
  ulpDump();

  // Wait
  delay(1000);
}