#pragma once
#include <stdint.h>
#include "tmc2209/tmc2209.hpp"
#include "tmc2209/step.hpp"

// LimitSwitch Pins
#define LIMIT0_PIN 10
#define LIMIT1_PIN 11
#define LIMIT2_PIN 12
#define LIMIT3_PIN 13

// LimitSwich Physical Wiring
// DONT TOUCH THIS
#define LIMIT_X0 LIMIT1_PIN
#define LIMIT_X1 LIMIT3_PIN
#define LIMIT_Y0 LIMIT2_PIN
#define LIMIT_Y1 LIMIT0_PIN

// Motor Directions
#define X_DIRECTION_TOP    0b01
#define X_DIRECTION_BOTTOM 0b00
#define Y_DIRECTION_LEFT   0b10
#define Y_DIRECTION_RIGHT  0b00

// Motor Mapping
// DONT TOUCH THIS
#define MOTOR_X 1
#define MOTOR_Y 0

// Homing Speeds
#define HOME_SPEED_FAST 10000
#define HOME_SPEED_SLOW 2000
#define HOME_SPEED_VERY_SLOW 500
#define OPERATING_SPEED 9000

enum HOME_AXIS_DIRECTION {
    TOP_LEFT     = X_DIRECTION_TOP | Y_DIRECTION_LEFT,
    TOP_RIGHT    = X_DIRECTION_TOP | Y_DIRECTION_RIGHT,
    BOTTOM_RIGHT = X_DIRECTION_BOTTOM | Y_DIRECTION_RIGHT,
    BOTTOM_LEFT  = X_DIRECTION_BOTTOM | Y_DIRECTION_LEFT
};

struct MotorPosition {
    int x;
    int y;
};

struct BoardMessurements {
    int x_padding;
    int y_padding;

    int x_num;
    int y_num;
    
    int cell_x;
    int cell_y;
};

BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y);
MotorPosition calcPosition(BoardMessurements * messurements, int x, int y);

void vTaskInitMotorHome(void *pvParameters);

#define HOME_COUNT 20
class CoreXYMaschine {
private:
    TMC2209_UART * uart_driver0;
    TMC2209_UART * uart_driver1;
    TMC2209_step_dual * step_driver;

    bool enableMotors = false;
    
    bool posUnknow = true;
    MotorPosition current_pos;

    BoardMessurements boardMessurements;
    SemaphoreHandle_t xMoveMutex;

    enum HOME_AXIS_DIRECTION ORIGIN = TOP_LEFT;

    int untilNextHome = HOME_COUNT;

public:
    CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver, uint core);
    
    void homeFast(MotorPosition * last_pos, bool ignoreMutex);
    void home(MotorPosition * last_pos);

    void initLimitSW();
    void debugPrintSwitch(uint32_t switchPin);
    void debugAllSwitchStates();

    void setMotorEnabled(bool enabled);
    bool getMotorEnabled();

    void setBoardMessurements(BoardMessurements messurements);
    void gotoPosition(int x, int y);
};

void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned int speed, unsigned int backupSpace, MotorPosition * last_pos);

extern absolute_time_t debounce_cooldown_ticks;
extern absolute_time_t limit_debounce_tick;
void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask);