#include "motor_drive.h"

void motor_init()
{
}

void motor_encoder_init()
{
}

bool motor_cmd_set_position(int speed_level, double position, int direction)
{
    int last_direction = direction;
    if (!motor_validation_set_position_parameters(speed_level, position, direction))
    {
        return false;
    }

    if (direction == 0)
    {
        last_direction = motor_find_short_path_direction(position);
    }

    motor_set_position(speed_level, position, last_direction);
    return true;
}

void motor_set_position(int speed_level, double position, int direction)
{
}

bool motor_validation_set_position_parameters(int speed_level, double position, int direction)
{
    return true;
}

int motor_find_short_path_direction(double position)
{
    double encoder_position = 0.0;
    encoder_position = motor_read_encoder();
    if (encoder_position > position)
    {
        return 2;
    }
    else
    {
        return 1;
    }
    return -1;
}

double motor_read_encoder()
{
    return 0.0;
}