/*
 * HipEncoder.cpp
 *
 *  Created on: 26 февр. 2020 г.
 *      Author: user
 */

#include "HipEncoder.hh"

struct IsPositionEqual {
    const float max_delta;

    const driver::PositionComponents & first;
    const driver::PositionComponents & second;

    IsPositionEqual(const driver::PositionComponents & first, const driver::PositionComponents & second, float max_delta) :
        first(first), second(second), max_delta(max_delta) {}

    //todo придумать сравнение получше
    operator bool() {

        return  first.sin - second.sin > - max_delta and first.sin - second.sin < max_delta and
                first.cos - second.cos > - max_delta and first.cos - second.cos < max_delta and
                first.increment_position == second.increment_position;

    }

};

static const float delta_equal = 20.f;

void driver::HipEncoder::process() {

    if( failure = not mixer.isValid() ) {

        ready = false;
        mixer.reset();

        work_state = request_hip_position;  //переход в режим калибровки

    }

    switch( work_state ) {
    case request_hip_position: {

        if( hip_position.sendRequest() ) work_state = wait_position_ready;

    } break;
    case wait_position_ready: {

        if( hip_position.isAnswerReady() ) {

            if( hip_position.isOk() ) {

                (void) mixer.calculatePosition();
                const PositionComponents current_position = mixer.getPositionComponents();

                prev_position = current_position;
                prev_absolute_position = hip_position.getPositionCode();

                work_state = request_next_hip_position;

            } else work_state = request_hip_position;

        }

    } break;
    case request_next_hip_position: {

        if( hip_position.sendRequest() ) work_state = wait_next_hip_position;

    } break;
    case wait_next_hip_position: {

        if( hip_position.isAnswerReady() ) {

            if( hip_position.isOk() ) {

                const unsigned long absolute_position = hip_position.getPositionCode();

                (void) mixer.calculatePosition();
                const PositionComponents current_position = mixer.getPositionComponents();

                if( IsPositionEqual( current_position, prev_position, delta_equal ) and
                        prev_absolute_position == absolute_position ) {

                    work_state = normal_work;

                    mixer.setAbsolutePosition( absolute_position );
                    position.write( mixer.calculatePosition() );

                    ready = true;

                } else {

                    prev_position = current_position;
                    prev_absolute_position = absolute_position;

                    work_state = request_next_hip_position;

                }

            } else work_state = request_next_hip_position;

        }

    } break;
    case normal_work: {

        position.write( mixer.calculatePosition() );

        scrt_valid = mixer.isScrtValid();
        phase_error = mixer.isPhaseError();

        {
            const PositionComponents current_position = mixer.getPositionComponents();

            sin_value = current_position.sin;
            cos_value = current_position.cos;
            inc_counter = current_position.increment_position;
        }


    } break;
    }



}

driver::HipEncoder::HipEncoder(IResolver& resolver,
                               peripheral::ICounter& counter,
                               HiperfaceNetworkDriver& hip_driver,
                               unsigned short address,
                               unsigned long sin_period_resolution,
                               unsigned long max_turn_value,
                               unsigned short resolver_hyst, unsigned short scrt_hyst,
                               float sector_sync_deviation ) :
                                       mixer(counter, sin_period_resolution, max_turn_value, resolver, resolver_hyst, scrt_hyst, sector_sync_deviation ),
                                       hip_position( hip_driver, address ) {
    PositionData data;

    data.angle = NAN;
    data.turn = 0;

    position.write( data );

}

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

    PositionData data = position.read();
    return ready ? std::pair<float, float>( data.turn, data.angle ) : std::pair<float, float>( 0, NAN );

}