/*
 * IEncoder.cpp
 *
 *  Created on: 1 февр. 2022 г.
 *      Author: titov
 */

#include "IEncoder.hh"

#include "../math/math_inc.hh"

std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::IEncoder::delta(
        std::pair<Turn, Angle> point_0, std::pair<Turn, Angle> point_1 ) {

    Angle point_1_angle_on_point_0_turn = in_turn( point_1, point_0.first );
    point_0.second -= point_1_angle_on_point_0_turn;
    return normalize( point_0 );

}

driver::IEncoder::Angle driver::IEncoder::in_turn(std::pair<Turn, Angle> point, Turn base) {

    Turn delta = point.first - base;

    return point.second + delta * math::constants::pi2;

}

std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::IEncoder::normalize( std::pair<Turn, Angle> point ) {

    using namespace std;

    while( point.second > math::constants::pi2 and isfinite(point.second) ) {

        point.second = fmaxf( point.second - math::constants::pi2, 0.0f );
        point.first += 1;

    }

    while( point.second < 0.0f and isfinite(point.second) ) {

        point.second = fminf( point.second + math::constants::pi2, math::constants::pi2 );
        point.first -= 1;

    }

    return point;

}

std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> stable_normalize( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> point ) {
    short extra_turn = point.second / math::constants::pi2;
    driver::IEncoder::Angle angle_in_turn = point.second - extra_turn * math::constants::pi2;

    if( angle_in_turn < 0.0f ) {
        extra_turn -= 1;
        angle_in_turn += math::constants::pi2;
    }

    return std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>( extra_turn, angle_in_turn );

}