/*
 * PositionMixerv2.cpp
 *
 *  Created on: 20 июл. 2020 г.
 *      Author: user
 */

#include "PositionMixerv2.hh"

driver::PositionCode driver::PositionMixer_v2::cross_correction(const PositionCode & position, const float period_angle ) {
    //угол в первом квадранте
    const bool in_pos_sector1 = (position.angle_data & 0x3) == 0;

    //угол в четвертом квадранте
    const bool in_pos_sector4 = (position.angle_data & 0x3) == 3;

    const bool period_angle_sector_34 = period_angle > math::constants::pi;

    const bool period_angle_sector_12 = period_angle <= math::constants::pi;

    if( period_angle_sector_34 && in_pos_sector1 )
        return decSinCosPeriod(position);

    if( period_angle_sector_12 && in_pos_sector4 )
        return incSinCosPeriod(position);

    return position;
}

void driver::PositionMixer_v2::sector_synchronization(
        const PositionCode & position, float period_angle ) {

    if( not is_sector_synchronized ) {

        unsigned long counter_on_synchronization = increment_counter.position_to_increments(position);

        if( counter_before_synchronized and counter_before_synchronized != counter_on_synchronization ) {

            short code_sector = static_cast<short>(position.angle_data & 0x3);

            if( counter_on_synchronization > counter_before_synchronized )
                period_angle += math::constants::pi_2;

            short angle_sector = -1;

            for( int i = 0; i < 4; ++i ) {

                float base_point = math::constants::pi_2 * i + math::constants::pi_2;

                if( period_angle >= base_point - sector_deviation and
                        period_angle < base_point + sector_deviation )
                    angle_sector = i;

            }

            if( angle_sector != -1 ) {

                sector_offset = angle_sector
                        - code_sector;

                is_sector_synchronized = true;

            }

        } else

            counter_before_synchronized = counter_on_synchronization;

    }

}

driver::PositionData driver::PositionMixer_v2::calculatePosition() {

    PositionCode position = increment_counter;
    const PositionComponents pos_components( resolver.getSinChannel(), resolver.getCosChannel(), increment_counter.position_to_increments(position) );

    last_position = pos_components;

    const PositionCode offset = { sector_offset, max_turn_value / 2 };
    position = PositionCode::applyOffset(position, offset, line_in_turn, max_turn_value);

    if( pos_components ) {

        if( resolver_valid_value_count < resolver_hyst )
            ++resolver_valid_value_count;
        else {
            scrt_valid = true;
        }

        resolver_invalid_value_count = 0;

    } else {

        if( resolver_invalid_value_count < counter_hyst )
            ++resolver_invalid_value_count;
        else {
            scrt_valid = false;
        }

        resolver_valid_value_count = 0;
    }


    float angle = increment_counter.position_to_rad(position.angle_data);
    unsigned short turn = position.turn;

    if( scrt_valid ) {

        if( pos_components ) {

            //atan2 выдает результат в диапазоне [-pi, +pi]
            const float atng = std::atan2(pos_components.sin, pos_components.cos);

            //приводим результат в диапазон [0, 2*pi]
            const float period_angle = atng < 0 ? (atng + math::constants::pi2) : atng;

            sector_synchronization( position, period_angle );

            if( is_sector_synchronized ) {

                short code_sector = static_cast<short>(position.angle_data & 0x3);
                if( code_sector == 0 or code_sector == 3 and pos_components.cos < 0.0f )
                    phase_error_flag = true;

                //Коррекция: инкрементируем или декрементируем один отсчет при несоответствии секторов аналогового и счетного интерфейсов.
                position = cross_correction( position, period_angle );

                angle = increment_counter.position_to_rad(
                                    position.angle_data & (static_cast<unsigned long>(-1) << line_in_period_power)
                                )
                                + period_angle * resolver_to_rad
                                - half_increment;
                turn = position.turn;

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

            }

        } else {

            angle = NAN;
            turn = position.turn;

        }

    }

    return { .angle = angle, .turn = turn };

}

driver::PositionComponents driver::PositionMixer_v2::getPositionComponents() const {

    return last_position;

}

void driver::PositionMixer_v2::setAbsolutePosition(
        unsigned long absolute_position) {

    const driver::PositionCode position_code = {
                  .angle_data = (absolute_position & ((1 << step_per_turn) - 1)) >> (periods_in_turn_power - line_in_period_power),
                  .turn = ( absolute_position >> step_per_turn ) % max_turn_value
        };

    increment_counter.setCounter(position_code);
    sector_resynchronizarion();
}

driver::PositionMixer_v2::PositionMixer_v2(peripheral::ICounter &counter,
                                           unsigned long sin_period_resolution,
                                           unsigned long max_turn_value,
                                           driver::IResolver &resolver,
                                           unsigned short resolver_hyst, unsigned short counter_hyst, float analog_sector_deviation )
: increment_counter(counter, 1 << (sin_period_resolution + line_in_period_power), max_turn_value), resolver(resolver),
  resolver_to_rad(1.f/(1 << sin_period_resolution)), step_per_turn(sin_period_resolution + periods_in_turn_power),
  max_angle_code((1 << (sin_period_resolution + line_in_period_power)) - 1),
  line_in_turn(1 << (sin_period_resolution + line_in_period_power)), max_turn_value(max_turn_value), resolver_hyst(resolver_hyst), counter_hyst(counter_hyst),
  half_increment( increment_counter.position_to_rad(1) * 0.5f ), sector_deviation(analog_sector_deviation) {}

driver::PositionCode driver::PositionMixer_v2::incSinCosPeriod(PositionCode code) {

    if(code.angle_data > max_angle_code - 1) {

        code.angle_data = 0;
        code.turn++;
        code.turn = increment_counter.toCorrectPositionCode(code).turn;

    } else {

        code.angle_data+= 1;

    }

    return code;

}

driver::PositionCode driver::PositionMixer_v2::decSinCosPeriod(PositionCode code) {

    if(code.angle_data < 1) {

        code.angle_data = max_angle_code;
        code.turn--;
        code.turn = increment_counter.toCorrectPositionCode(code).turn;

    } else {

        code.angle_data-= 1;

    }

    return code;
}

void driver::PositionMixer_v2::sector_resynchronizarion() {
    is_sector_synchronized = false;
    counter_before_synchronized = 0;
    sector_offset = 0;
}

void driver::PositionMixer_v2::reset() {
    increment_counter.resetCounter();
    sector_resynchronizarion();
    resolver_valid_value_count = 0;
}