#include "matrix.h"
#include "joystick.h"

// Define two tasks for Blink & AnalogRead.
void TaskJoystickREAD(void *pvParameters);
void TaskHandleGame(void *pvParameters);
void TaskMatrixWrite(void *pvParameters);
TaskHandle_t task_joystick_read_handle; 
TaskHandle_t task_matrix_write_handle; 
TaskHandle_t task_game_handle;

const int QueueElementSize = 10;
QueueHandle_t from_joy_to_game_queue_handle;

bool punch = false;

void change_punch(){
  punch = true;
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S2!");
  pinMode(JOY_SELECT, INPUT_PULLUP);
  
  attachInterrupt(digitalPinToInterrupt(JOY_SELECT), change_punch, FALLING);

  matrix_init();

  xTaskCreate(TaskMatrixWrite,"TaskMatrixWrite", 2048,
              NULL, 1, &task_matrix_write_handle);
  xTaskCreate(TaskHandleGame,"TaskHandleGame", 2048,
              NULL, 1, &task_game_handle);
  xTaskCreate(TaskJoystickREAD,"TaskJoystickREAD", 2048,
              NULL, 1, &task_joystick_read_handle);

  from_joy_to_game_queue_handle = xQueueCreate(QueueElementSize, 
    sizeof(point_t));
  
    if (from_joy_to_game_queue_handle == NULL) {
    Serial.println("Queue could not be created. Halt.");
    while (1) {
      delay(1000);  
      }
    }

}

void loop() {
  static bool state = false;
  if (punch) {
    state = !state;
    if(state) {
      vTaskSuspend(task_joystick_read_handle);
    }
    else {
      vTaskResume(task_joystick_read_handle);
    }
    punch = false;
  }
 delay(1000); // this speeds up the simulation
}

void TaskMatrixWrite(void *pvParameters){
  while(true){   
    show();
    vTaskDelay(200);
  }
}

void TaskJoystickREAD(void *pvParameters){
  point_t new_point;
  while(true){
    read_place(&new_point);
      int ret = xQueueSend(from_joy_to_game_queue_handle, (void *)&new_point, 0);
      if (ret == pdTRUE) {
        // The message was successfully sent.
      } else if (ret == errQUEUE_FULL) {
        Serial.println("The `TaskReadFromSerial` was unable to send data into the Queue");
      }  
    vTaskDelay(200);
  }
}

void TaskHandleGame(void *pvParameters){
  static point_t point;
  while(true){
    if (from_joy_to_game_queue_handle != NULL){
      if (xQueueReceive(from_joy_to_game_queue_handle, &point, 0)){
        if (point.x == 0) {
          shift(-1,0);
        } else if (point.x > 4096){
          shift(1,0);
        }

        if (point.y == 0) {
          shift(0,1);
        } else if (point.y > 4096){
          shift(0,-1);
        }

      }
    }
    vTaskDelay(100);
  }
}