#include "CoreXY.hpp"

#include "FreeRTOS.h"
#include "task.h"

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

    uint8_t x_dir =  direction & 0b01;
    uint8_t y_dir = (direction & 0b10) >> 1;

    uint8_t x_dir_inv = x_dir ^ 1;
    uint8_t y_dir_inv = y_dir ^ 1;

    uint8_t x_inv_limit = x_dir_inv == X_DIRECTION_BOTTOM ? LIMIT_X0 : LIMIT_X1;
    uint8_t y_inv_limit = y_dir_inv == Y_DIRECTION_LEFT ? LIMIT_Y0 : LIMIT_Y1;
    bool backup_x = !gpio_get(x_inv_limit) && (backupSpace > 0);
    bool backup_y = !gpio_get(y_inv_limit) && (backupSpace > 0);

    // Start Backup if necessary
    if(backup_x) {
        if(MOTOR_X) {
            driver->set_conf1(speed, x_dir_inv);
            driver->pulse1(backupSpace);
        } else {
            driver->set_conf0(speed, y_dir_inv);
            driver->pulse0(backupSpace);
        }
    }

    if(backup_y) {
        if(MOTOR_Y) {
            driver->set_conf1(speed, y_dir_inv);
            driver->pulse1(backupSpace);
        } else {
            driver->set_conf0(speed, y_dir_inv);
            driver->pulse0(backupSpace);
        }
    }


    // Wait for end of Backup
    if(backup_x) {
        if(MOTOR_X) {
            driver->wait1(0);
        } else {
            driver->wait0(0);
        }
    }

    if(backup_y) {
        if(MOTOR_Y) {
            driver->wait1(0);
        } else {
            driver->wait0(0);
        }
    }

    vTaskDelay(100 / portTICK_PERIOD_MS);

    // Start Motors
    const unsigned int total_steps = 25000;
    // Set Configuration
    if(MOTOR_X) {
        driver->set_conf1(speed, x_dir);
    } else {
        driver->set_conf0(speed, x_dir);
    }

    if(MOTOR_Y) {
        driver->set_conf1(speed, y_dir);
    } else {
        driver->set_conf0(speed, y_dir);
    }

    // Start Motors
    if(MOTOR_X) {
        driver->pulse1(total_steps);
    } else {
        driver->pulse0(total_steps);
    }

    if(MOTOR_Y) {
        driver->pulse1(total_steps);
    } else {
        driver->pulse0(total_steps);
    }

    bool x_done = false;
    bool y_done = false;

    volatile int last_x = -1;
    volatile int last_y = -1;

    for(int i=0; i<5000; i++) {
        if(gpio_get(LIMIT_X0) || gpio_get(LIMIT_X1)) {
            x_done = true;
            if(MOTOR_X) {
                driver->pulse1(0);
            } else {
                driver->pulse0(0);
            }
        }

        if(gpio_get(LIMIT_Y0) || gpio_get(LIMIT_Y1)) {
            y_done = true;
            if(MOTOR_Y) {
                driver->pulse1(0);
            } else {
                driver->pulse0(0);
            }

        }

        if(x_done && y_done) {
            break;
        }

        vTaskDelay(1 / portTICK_PERIOD_MS);
    }

    // Emergency Stop
    if(!(x_done && y_done)) {
        driver->pulse0(0);
        driver->pulse1(0);
    }

    driver->wait0(0);
    driver->wait1(0);

    if(MOTOR_X) {
        if(last_pos != NULL)
            last_pos->x = driver->get_remaining_steps1();
    } else {
        if(last_pos != NULL)
            last_pos->x = driver->get_remaining_steps0();
    }

    if(MOTOR_Y) {
        last_pos->y = driver->get_remaining_steps1();
    } else {
        last_pos->y = driver->get_remaining_steps0();
    }

    if(last_pos != NULL)
        last_pos->x = total_steps - last_pos->x;
    
    if(last_pos != NULL)
        last_pos->y = total_steps - last_pos->y;
}