/*
 * PositionMixer.cpp
 *
 *  Created on: 2 мар. 2020 г.
 *      Author: user
 */

#include "PositionMixer.hh"

#include "../peripheral/ICounter.hh"

driver::PositionData driver::PositionMixer::getPosition() {

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

    float angle = NAN;
    //todo можно проверять сигналы резольвера более строго (sin^2 + cos^2)
    if( pos_components ) {
        const float atng = std::atan2(pos_components.sin, pos_components.cos);
        const float period_angle = atng > math::constants::pi ? (atng - math::constants::pi2) :
                (atng < - math::constants::pi ? (atng + math::constants::pi2) : atng);

        const short angle_code = position.angle_data & 0x3;

        if ( ( not ( period_angle < mistrust_angle and period_angle > -mistrust_angle ) )
                and ( ( period_angle < math::constants::pi_2 - mistrust_angle ) and ( period_angle > -math::constants::pi_2 + mistrust_angle ) ) ) {
            switch(angle_code) {
            case 0: {
                phase_error_flag = ( period_angle < 0 or period_angle > math::constants::pi_2 ) ? true : false;
            } break;
            case 1: {
                phase_error_flag = ( period_angle < math::constants::pi_2 ) ? true : false;
            } break;
            case 2: {
                phase_error_flag = ( period_angle > -math::constants::pi_2 ) ? true : false;
            } break;
            case 3: {
                phase_error_flag = ( period_angle < -math::constants::pi_2 or period_angle > 0 ) ? true : false;
            } break;
            }
        }

        if( period_angle < mistrust_angle and period_angle > -mistrust_angle
                and ( angle_code == 0 or angle_code == 3 ) ) {

            if( period_angle < 0.f )

                    angle = increment_counter.position_to_rad( position.angle_data )
                        + ( angle_code == 0 ? 0.0f : minus_zero_point_rad );

            else

                    angle = increment_counter.position_to_rad( position.angle_data )
                        + ( angle_code == 0 ? plus_zero_point_rad : minus_zero_rad );

        } else {
            //приведенное к диапазону 0..2*pi значение
            const float period_angle_pos = period_angle < 0.f ? (period_angle + math::constants::pi2) : period_angle;

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

            }

    } else {

        angle = increment_counter.position_to_rad(position.angle_data);

    }

    pos_code = position.angle_data & 0x3;

    return { .angle = angle, .turn = static_cast<unsigned short>( position.turn /*- turn*/ ) };

}

driver::PositionComponents driver::PositionMixer::getPositionComponents() {

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

}

void driver::PositionMixer::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
        };

    increment_counter.setCounter(position_code);

}