/*
 * EncoderQuadratureCounter.cpp
 *
 *  Created on: 22 янв. 2020 г.
 *      Author: user
 */

#include "EncoderQuadratureCounter.hh"

float driver::derail::EncoderQuadratureCounter::getAngle() const {

    return failure ? NAN : angle;

}

float driver::derail::EncoderQuadratureCounter::getTurn() const {

    return failure ? NAN : turn_counter;

}

bool driver::derail::EncoderQuadratureCounter::configure( Setting & set ) {

    if( set.line_in_turn == 0 or set.max_turn_value == 0 ) return false;

    max_turn_value = set.max_turn_value;
    line_in_turn = set.line_in_turn;

    _1_line_to_rad = math::constants::pi2 / line_in_turn;

    return true;

}

bool driver::derail::EncoderQuadratureCounter::isFailure() const {

    return failure;

}

void driver::derail::EncoderQuadratureCounter::process() {

    if( not (failure = counter.isError()) ) {

        long temp_position = angle_position + counter.getDeltaCounter();

        //todo плохо если temp_position < -line_in_turn...
        if( temp_position < 0 ) {
            temp_position += line_in_turn;

            if( turn_counter > 0 ) turn_counter--;
            else {
                turn_counter = max_turn_value - 1;
            }
        } else if( temp_position >= line_in_turn ) {
            temp_position -= line_in_turn;

            if( turn_counter < max_turn_value - 1 ) turn_counter++;
            else {
                turn_counter = 0;
            }
        }

        angle_position = temp_position;

        angle = _1_line_to_rad * angle_position;

        data.write( std::pair<float, float>( turn_counter, angle ) );
    }

}

driver::derail::EncoderQuadratureCounter::EncoderQuadratureCounter(
        peripheral::ICounter & counter) :
        counter(counter) {}

void driver::derail::EncoderQuadratureCounter::resetFailure() {

    counter.resetError();

    failure = counter.isError();

}

void driver::derail::EncoderQuadratureCounter::setAbsolurePosition(
        float angle, unsigned long turn ) {

    while( angle < 0.f ) angle += math::constants::pi2;
    while( angle >= math::constants::pi2 ) angle -= math::constants::pi2;

    angle_position = angle / _1_line_to_rad;

    while( turn >= max_turn_value ) turn -= max_turn_value;

    turn_counter = turn;

}

std::pair<float, float> driver::derail::EncoderQuadratureCounter::getPosition() const {

    return failure ? std::pair<float, float>( 0, NAN ) : data.read();

}