#include "CoreXY.hpp"

#include "pico/stdlib.h"
#include <stdio.h>

absolute_time_t debounce_cooldown_ticks = 5000;
absolute_time_t limit_debounce_tick = 0;

void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask) {
    absolute_time_t current_tick = get_absolute_time();
    if((current_tick - limit_debounce_tick) < debounce_cooldown_ticks) {
        return;
    } else {
        limit_debounce_tick = current_tick;
    }
}

void CoreXYMaschine::initLimitSW() {
    gpio_init(LIMIT_X0);
    gpio_init(LIMIT_X1);
    gpio_init(LIMIT_Y0);
    gpio_init(LIMIT_Y1);

    gpio_set_dir(LIMIT_X0, GPIO_IN);
    gpio_set_dir(LIMIT_X1, GPIO_IN);
    gpio_set_dir(LIMIT_Y0, GPIO_IN);
    gpio_set_dir(LIMIT_Y1, GPIO_IN);

    gpio_pull_up(LIMIT_X0);
    gpio_pull_up(LIMIT_X1);
    gpio_pull_up(LIMIT_Y0);
    gpio_pull_up(LIMIT_Y1);

    gpio_set_input_hysteresis_enabled(LIMIT_X0, true);
    gpio_set_input_hysteresis_enabled(LIMIT_X1, true);
    gpio_set_input_hysteresis_enabled(LIMIT_Y0, true);
    gpio_set_input_hysteresis_enabled(LIMIT_Y1, true);

    gpio_set_irq_enabled_with_callback(
        LIMIT_X0, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );

    gpio_set_irq_enabled_with_callback(
        LIMIT_X1, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );

    gpio_set_irq_enabled_with_callback(
        LIMIT_Y0, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );

    gpio_set_irq_enabled_with_callback(
        LIMIT_Y1, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );
}

void CoreXYMaschine::debugPrintSwitch(uint32_t switchPin) {
    printf("Switch: ");

    switch (switchPin) {
        case LIMIT_X0:
            printf("X0");
            break;
        case LIMIT_X1:
            printf("X1");
            break;
        case LIMIT_Y0:
            printf("Y0");
            break;
        case LIMIT_Y1:
            printf("Y1");
            break;
        default:
            printf("??");
            break;
    }

    switch(switchPin) {
        case LIMIT0_PIN:
            printf(" 0");
            break;
        case LIMIT1_PIN:
            printf(" 1");
            break;
        case LIMIT2_PIN:
            printf(" 2");
            break;
        case LIMIT3_PIN:
            printf(" 3");
            break;
    }

    printf(" %d", gpio_get(switchPin));

    printf(" (%d)\n", switchPin);
}

void CoreXYMaschine::debugAllSwitchStates() {
    printf("X0: %d\n", gpio_get(LIMIT_X0));
    printf("X1: %d\n", gpio_get(LIMIT_X1));
    printf("Y0: %d\n", gpio_get(LIMIT_Y0));
    printf("Y1: %d\n", gpio_get(LIMIT_Y1));
}
