#include "CoreXY.hpp"
#include "FreeRTOS.h"

void CoreXYMaschine::setMotorEnabled(bool enabled) {
    this->step_driver->set_enable0(enabled);
    this->step_driver->set_enable1(enabled);
    this->enableMotors = enabled;
}

bool CoreXYMaschine::getMotorEnabled() {
    return this->enableMotors;
}

void CoreXYMaschine::homeFast(MotorPosition * last_pos, bool ignoreMutex) {
    if(!ignoreMutex)
        xSemaphoreTake(xMoveMutex, portMAX_DELAY);

    homeXY(this->step_driver, ORIGIN, HOME_SPEED_FAST, 0, last_pos);
    posUnknow = false;
    current_pos = {x:0, y:0};
    untilNextHome = HOME_COUNT;
    
    if(!ignoreMutex)
        xSemaphoreGive(xMoveMutex);
}

void CoreXYMaschine::home(MotorPosition * last_pos) {
    xSemaphoreTake(xMoveMutex, portMAX_DELAY);
    MotorPosition res = {0, 0};

    homeXY(step_driver, ORIGIN, HOME_SPEED_FAST, 0, &res);
    homeXY(step_driver, ORIGIN, HOME_SPEED_SLOW, 500, &res);
    posUnknow = false;
    current_pos = {x:0, y:0};
    untilNextHome = HOME_COUNT;
    xSemaphoreGive(xMoveMutex);
}

void CoreXYMaschine::setBoardMessurements(BoardMessurements messurements) {
    this->boardMessurements = messurements;
}

void CoreXYMaschine::gotoPosition(int x, int y) {
    xSemaphoreTake(xMoveMutex, portMAX_DELAY);
    MotorPosition pos = calcPosition(&this->boardMessurements, x, y);

    if(posUnknow || untilNextHome == 0) {
        homeFast(&current_pos, true);

    } else {
        untilNextHome--;
    }

    if(!enableMotors) {
        setMotorEnabled(true);
    }

    int x_steps = pos.x - current_pos.x;
    int y_steps = pos.y - current_pos.y;

    current_pos.x = current_pos.x + x_steps;
    current_pos.y = current_pos.y + y_steps;

    // Direction Magic
    uint8_t x_dir = ( ORIGIN & 0b01      ) ^ 1;
    uint8_t y_dir = ((ORIGIN & 0b10) >> 1) ^ 1;

    if(x_steps < 0) {
        x_dir = x_dir ^ 1;
        x_steps = x_steps * -1;
    }

    if(y_steps < 0) {
        y_dir = y_dir ^ 1;
        y_steps = y_steps * -1;
    }

    if(MOTOR_X) {
        step_driver->set_conf1(OPERATING_SPEED, x_dir);
    } else {
        step_driver->set_conf0(OPERATING_SPEED, x_dir);
    }

    if(MOTOR_Y) {
        step_driver->set_conf1(OPERATING_SPEED, y_dir);
    } else {
        step_driver->set_conf0(OPERATING_SPEED, y_dir);
    }

    if(MOTOR_X) {
        step_driver->pulse1(x_steps);
    } else {
        step_driver->pulse0(x_steps);
    }

    if(MOTOR_Y) {
        step_driver->pulse1(y_steps);
    } else {
        step_driver->pulse0(y_steps);
    }

    step_driver->wait0(0);
    step_driver->wait1(0);
    xSemaphoreGive(xMoveMutex);
}