#include "CoreXY.hpp"

#include <math.h>

BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y) {
    BoardMessurements res;

    MotorPosition * smaller;
    MotorPosition * bigger;

    if(p0.x < p1.x) {
        smaller = &p0;
        bigger = &p1;
    } else {
        smaller = &p1;
        bigger = &p0;
    }

    res.x_num = grid_x;
    res.y_num = grid_y;

    int x_total = bigger->x - smaller->x;
    int y_total = bigger->y - smaller->y;

    res.cell_x = x_total / (grid_x-1);
    res.cell_y = y_total / (grid_y-1);

    res.x_padding = smaller->x;
    res.y_padding = smaller->y;

    return res;
}

MotorPosition calcPosition(BoardMessurements * messurements, int x, int y) {
    MotorPosition res;

    res.x = messurements->x_padding + (x * messurements->cell_x);
    res.y = messurements->y_padding + (y * messurements->cell_y);

    return res;
}