#define GPIO_LED 12
#define GPIO_BUTTON_L 25
#define GPIO_BUTTON_R 26

static QueueHandle_t queue;

//Debounce Task
static void debounce_task(void *argp){
  unsigned int button_gpio= *(unsigned*)argp;
  uint32_t level, state=0;
  uint32_t mask= 0x7FFFFFFF;
  int event, last=-999;
  for(;;){
    level = !digitalRead(button_gpio);
    state =(state<<1)|level;
    if((state&mask)==mask){
        event=button_gpio;
      }else{
        event=-button_gpio;
      }
    if (event!= last){
      if(xQueueSendToBack(queue,&event,1)== pdPASS){
        last=event;
        }
    }
    taskYIELD();
  }
}

//LED Task
static void  press_task(void*argp){
  static const uint32_t enable=(1<<GPIO_BUTTON_R)|(1<<GPIO_BUTTON_L);
  BaseType_t s;
  int event;
  uint32_t state = 0;
  digitalWrite(GPIO_LED,LOW);
  for(;;){
    s =xQueueReceive(
      queue,
      &event,
      portMAX_DELAY
    );
    
    assert(s == pdPASS);
    if(event>=0){
      //button pressed 

      state|= 1<<event;
    }else{
      
    state&= ~(1<<-event);
    }
    printf("state  ");
    printf("%x",state);
    printf("\n");
    printf("enable ");
    printf("%x",enable);
    printf("\n");
    if (state==enable){
      digitalWrite(GPIO_LED,HIGH);
    }else{
      digitalWrite(GPIO_LED,LOW);  
    }
    vTaskDelay(1000);
  }
}

void setup() {
  int app_cpu=xPortGetCoreID();
  static int left= GPIO_BUTTON_L;
  static int right= GPIO_BUTTON_R;

  TaskHandle_t h;
  BaseType_t rc;
  delay(500);
  queue=xQueueCreate(2,sizeof(int));
  assert(queue);

  pinMode(GPIO_LED, OUTPUT);
  pinMode(GPIO_BUTTON_L, INPUT_PULLUP);
  pinMode(GPIO_BUTTON_R, INPUT_PULLUP);
  printf("creating Task debouncing Left\n");
  rc= xTaskCreatePinnedToCore(
    debounce_task,
    "debounce left",
    2048,
    &left,
    1,
    &h,
    app_cpu
  );
  assert(rc==pdPASS);
  assert(h);
  
  printf("creating Task debouncing Right\n");
  rc= xTaskCreatePinnedToCore(
    debounce_task,
    "debounce right",
    2048,
    &right,
    1,
    &h,
    app_cpu
  );
  assert(rc==pdPASS);
  assert(h);
  
  printf("creating Task press\n");
  rc= xTaskCreatePinnedToCore(
    press_task,
    "press_task",
    2048,
    nullptr,
    1,
    &h,
    app_cpu
    );
    
  assert(rc==pdPASS);
  assert(h);

}

void loop() {
  // put your main code here, to run repeatedly:
vTaskDelete(nullptr);
}