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

#ifndef UMLIBRARY_DRIVER_HIPENCODER_HH_
#define UMLIBRARY_DRIVER_HIPENCODER_HH_

#include "IEncoder.hh"
#include "IResolver.hh"

#include "IncrementCounter.hh"
#include "PositionMixer.hh"
#include "PositionMixerv2.hh"

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

#include "../communication/hiperface/HipPosition.hh"

#include "../math/math_inc.hh"
#include "../common/DoubleBuffer.hpp"

namespace driver {

class HipEncoder: public IEncoder {
private:
    communication::hiperface::HipPositionClient hip_position;
    PositionMixer_v2 mixer;

    PositionComponents prev_position;           //предыдущее сохраненное положение по аналоговому и квадратурному сигналам
    unsigned long prev_absolute_position = 0;   //предыдущее сохраненное положение по цифровому интерфейсу

    DoubleBuffer<PositionData> position;

    bool ready = false;
    bool failure = false;

    enum SynchronizationState {
        request_hip_position,
        wait_position_ready,
        request_next_hip_position,
        wait_next_hip_position,
        normal_work
    };

    SynchronizationState work_state = request_hip_position;

    //todo для отладки
    float phase_error;

    //todo для отладки
    float atan;

    //todo для отладки
    float pos_code;

    float sin_value = 0.0f;
    float cos_value = 0.0f;
    float inc_counter = 0.0f;

    bool scrt_valid = false;
    bool sync_error = false;

public:

    float getAngle() const override     { return ready ? position.read().angle : NAN; }
    float getTurn() const override      { return ready ? position.read().turn : NAN; }

    std::pair<float, float> getPosition() const;

    float getAbsoluteAngle() const      {
        PositionData data = position.read();
        return ready ? data.angle + data.turn*math::constants::pi2 : NAN;
    }

    const bool & isReady() const        { return ready; }

    const bool & isFailure() const      { return failure; }

    const bool & isScrtValid() const    { return scrt_valid; }
    const bool & isSyncError() const    { return sync_error; }

    void process();

    const float & getSin() const        { return sin_value; }

    const float & getCos() const        { return cos_value; }

    const float & getCounter() const    { return inc_counter; }

    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 );

};

} /* namespace driver */

#endif /* UMLIBRARY_DRIVER_HIPENCODER_HH_ */