/* Week3 Queue */
#define GPIO_LED 12
#define GPIO_BUTTONL 25
#define GPIO_BUTTONM 26
#define GPIO_BUTTONR 27

static QueueHandle_t queue;           // ตัวแปรคิวสำหรับเก็บเหตุการณ์
static const int reset_press = -998;  // ค่าที่ใช้ในการรีเซ็ตการกดปุ่ม

/* ฟังก์ชัน debouncing สำหรับการกดปุ่ม */
static void debounce_task(void *argp) {
    unsigned button_gpio = *(unsigned*)argp;  // รับพินของปุ่ม
    uint32_t level, state = 0;                // ตัวแปรสำหรับสถานะของปุ่ม
    uint32_t mask = 0x7FFFFFFF;               // ใช้สำหรับตรวจจับการกดปุ่มหลายครั้ง
    int event, last = -999;                   // ตัวแปรเก็บเหตุการณ์ และสถานะล่าสุด

    for (;;) {
        level = !digitalRead(button_gpio);   // อ่านสถานะปุ่ม (กด = 0, ปล่อย = 1)
        state = (state << 1) | level;        // เลื่อนบิตเพื่อเก็บสถานะการกดปุ่ม

        /* ถ้าปุ่มกดติดต่อกันเป็นเวลานาน จะถือว่าเป็นการกดปุ่ม */
        if ((state & mask) == mask)
            event = button_gpio;    // เหตุการณ์กดปุ่ม
        else
            event = -button_gpio;   // เหตุการณ์ปล่อยปุ่ม

        /* ตรวจสอบข้อมูลในคิวก่อนการเพิ่มข้อมูลใหม่ */
        int peeked_event;
        
        BaseType_t status = xQueuePeek(queue, &peeked_event, 0);    // แสดงคิวที่อยู่หน้าสุด
        UBaseType_t messages = uxQueueMessagesWaiting(queue);       // แสดงจำนวนเหตุการณ์ในคิว
        UBaseType_t spaces = uxQueueSpacesAvailable(queue);         // แสดงจำนวนคิวที่ว่างอยู่

        if (status == pdPASS) {
            /* ถ้ามีข้อมูลในคิวแล้ว ก็ไม่เพิ่มข้อมูลใหม่เข้าไป */
            printf("Peeked event: %d, Messages in Queue : %d, Space in Queue : %d\n", peeked_event, messages, spaces);
        } else {
            /* ถ้าคิวว่างหรือไม่สามารถ peek ข้อมูลได้ */
            if (event != last) {                                      // ถ้าเหตุการณ์ไม่เหมือนครั้งก่อน
                if (xQueueSendToBack(queue, &event, 0) == pdPASS) {   // ส่งเหตุการณ์ไปยังคิว
                    last = event;                                     // อัพเดตสถานะล่าสุด
                } else if (event < 0) {                               // ถ้าคิวเต็มและเป็นเหตุการณ์ปล่อยปุ่ม
                    /* รีเซ็ตคิวแล้วส่งเหตุการณ์ reset_press ไปคิว */
                    do {
                        xQueueReset(queue);                                         // รีเซ็ตคิว
                    } while (xQueueSendToBack(queue, &reset_press, 0) != pdPASS);   // ส่ง reset_press ไปคิว
                    last = event;                                                   // อัพเดตสถานะล่าสุด
                }
            }
        }
        taskYIELD();    // ให้เวลาให้ task อื่นทำงาน
    }
}

/* ฟังก์ชันสำหรับการควบคุมเครื่องกด (Hydraulic Press) */
static void press_task(void *argp) {
    static const uint32_t enable = (1 << GPIO_BUTTONL) | (1 << GPIO_BUTTONM) | (1 << GPIO_BUTTONR);   // กำหนดค่าที่ทั้งสามปุ่มต้องกดเพื่อเปิดไฟ
    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 == reset_press) {                         // ถ้าเหตุการณ์คือ reset_press
            digitalWrite(GPIO_LED, LOW);                    // ปิดไฟ
            state = 0;                                      // รีเซ็ตสถานะ
            printf("RESET!!\n");
            continue;
        }

        /* ถ้าเหตุการณ์คือการกดปุ่ม */
        if (event >= 0) {
            state |= 1 << event;        // ตั้งค่า state ให้มีบิตที่ตรงกับปุ่มที่กด
        } else {                        /* ถ้าเหตุการณ์คือการปล่อยปุ่ม */
            state &= ~(1 << -event);    // เคลียร์บิตที่ตรงกับปุ่มที่ปล่อย
        }

        /* ถ้าทั้งสามปุ่มถูกกดพร้อมกัน ให้เปิดไฟ */
        if (state == enable) {
            digitalWrite(GPIO_LED, HIGH);   // เปิดไฟ
        } else {
            digitalWrite(GPIO_LED, LOW);    // ปิดไฟ
        }
    }
}

void setup() {
    int app_cpu = xPortGetCoreID();   // ตรวจสอบ CPU ที่ใช้งาน
    static int left = GPIO_BUTTONL;   // กำหนดพินของปุ่มซ้าย
    static int mid = GPIO_BUTTONM;    // กำหนดพินของปุ่มกลาง
    static int right = GPIO_BUTTONR;  // กำหนดพินของปุ่มขวา
    TaskHandle_t h;
    BaseType_t rc;

    delay(2000);    // หน่วงเวลา 2 วินาทีเพื่อให้ USB เชื่อมต่อ

    /* สร้างคิวที่มีความยาว 2 ช่อง*/
    queue = xQueueCreate(3, sizeof(int));
    assert(queue);

    pinMode(GPIO_LED, OUTPUT);            // กำหนดพิน LED เป็น OUTPUT
    pinMode(GPIO_BUTTONL, INPUT_PULLUP);  // กำหนดพินปุ่มซ้ายเป็น INPUT_PULLUP
    pinMode(GPIO_BUTTONM, INPUT_PULLUP);  // กำหนดพินปุ่มกลางเป็น INPUT_PULLUP
    pinMode(GPIO_BUTTONR, INPUT_PULLUP);  // กำหนดพินปุ่มขวาเป็น INPUT_PULLUP

    /* สร้าง task สำหรับ debounce ปุ่มซ้าย */
    rc = xTaskCreatePinnedToCore(debounce_task, "debounceL", 2048, &left, 1, &h, app_cpu);
    assert(rc == pdPASS);
    assert(h);

    /* สร้าง task สำหรับ debounce ปุ่มกลาง */
    rc = xTaskCreatePinnedToCore(debounce_task, "debounceM", 2048, &mid, 1, &h, app_cpu);
    assert(rc == pdPASS);
    assert(h);

    /* สร้าง task สำหรับ debounce ปุ่มขวา */
    rc = xTaskCreatePinnedToCore(debounce_task, "debounceR", 2048, &right, 1, &h, app_cpu);
    assert(rc == pdPASS);
    assert(h);

    /* สร้าง task สำหรับการควบคุมเครื่องกด */
    rc = xTaskCreatePinnedToCore(press_task, "led", 2048, nullptr, 1, &h, app_cpu);
    assert(rc == pdPASS);
    assert(h);
}

void loop() {
    vTaskDelete(nullptr);  // ลบ task void loop
}