#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

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;
};

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


void initLimitSwitches();
void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask);

void waitForSwitch(uint32_t switchPin);

void debugPrintSwitch(uint32_t switchPin);
void printAllSwitchStates();