#include "EncoderMultiturnWraper.hh"

bool driver::detail::EncoderMultiturnWraper::configure(Setting &config)
{

    bool result;
    if( result = config.isValid() ) {
        turn_counter.setMaxCount(config.num_turn);
        turn_counter = 0;
        max_angle_delta = config.max_angle_delta;
    };

    return result;

}

driver::detail::EncoderMultiturnWraper::TurnCounter::operator float() { return num_count; }

driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator++() {
    if(not(++num_count < max_count)) {
        num_count = 0;
    }

    return *this;
}

driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator--() {
    if(not(--num_count < max_count)) {
        num_count = max_count - 1;
    }

    return *this;
}

driver::detail::EncoderMultiturnWraper::TurnCounter & driver::detail::EncoderMultiturnWraper::TurnCounter::operator=(unsigned short new_num_count) {
    num_count = new_num_count;

    while(not(num_count < max_count)) num_count -= max_count;

    return *this;
}

void driver::detail::EncoderMultiturnWraper::TurnCounter::setMaxCount(unsigned short new_max_count) {
    max_count = new_max_count;

    while(not(num_count < max_count)) num_count -= max_count;
}


void driver::detail::EncoderMultiturnWraper::process() {

    using namespace std;

    float angle = single_turn_encoder.getAngle();

    if( isfinite(angle) ) {

        if( isfinite(prev_angle) ) {

            float angle_delta = angle - prev_angle;

            if( angle_delta > math::constants::pi ) {

                --turn_counter;
                angle_delta -= math::constants::pi2;

            } else if( angle_delta < - math::constants::pi ) {

                ++turn_counter;
                angle_delta += math::constants::pi2;

            }

            if( fabsf(angle_delta) > max_angle_delta )
                ++error_counter;

            else
                error_counter = 0;

        }

        prev_angle = angle;

    } else
        ++error_counter;

    data.write( std::pair<Turn, Angle>( turn_counter, angle ) );

}

void driver::detail::EncoderMultiturnWraper::EncoderMultiturnWraper::reset() {

    error_counter = 0;
    prev_angle = NAN;
    turn_counter = 0;
    data.write( std::pair<Turn, Angle>( 0, NAN ) );

}

driver::detail::EncoderMultiturnWraper::EncoderMultiturnWraper(
        IEncoder & encoder, unsigned short max_error ) : single_turn_encoder(encoder), error_counter(max_error) {

}