/*
 * EncoderSsi.h
 *
 *  Created on: 20 мар. 2019 г.
 *      Author: krugliy
 */

#ifndef SOURCE_DRIVER_ENCODERSSI_H_
#define SOURCE_DRIVER_ENCODERSSI_H_

#include <cmath>
#include <stdint.h>

#include "IEncoder.hh"

#include "../common/DoubleBuffer.hpp"

namespace driver { namespace detail {

struct EncoderSsiConfig {
    struct Config {

        uint16_t num_error;                 //!<Количество ошибок, при котором формируется отказ отказа.
        float max_angle_delta;              //!<Изменение между двумя значениями угла больше данного порога считаются ошибкой.

        uint16_t ssi_data_bit_len;    //!<Количество бит, которые передает энкодер.
        uint16_t max_turn_bit;        //!<Количество бит, используемых для кода оборотов.

        //аппаратные настройки:
        uint16_t frame_size;          //!<Размер фрейма в настройках spi,
        //чтобы не потерять младшие биты от энкодера, должен быть не меньше чем ssi_data_bit_len

        uint16_t ssi_extra_bit;       //!Старшие незначащие биты принятые из-за настроек spi
        uint16_t shift_count;               //!<Сдвиг данных в принятом кадре. Учитывает задержки в линии данных относительно clk.
        bool inverse;                       //!<Признак инверсии принятых данных.

        bool swap_bytes;                //!< Признак необходимости менять местами байты в принятых данных.
    };

    EncoderSsiConfig( Config config ) : error_counter(config.num_error), max_angle_delta(config.max_angle_delta),
            turn_bit_mask( ( 1 << config.max_turn_bit ) - 1 ),
            shift_count(config.frame_size - config.ssi_data_bit_len - ( config.ssi_extra_bit + config.shift_count )),
            inverse(config.inverse), swap_bytes( config.swap_bytes )  {}

    struct ErrorCounter {

        typedef unsigned short ErrorCount;

        ErrorCount counter;
        ErrorCount max_error;

        ErrorCounter( ErrorCount num_error ) : counter(1), max_error(num_error) {}

    } error_counter;

    float max_angle_delta;
    uint32_t turn_bit_mask;

    uint16_t shift_count;
    bool inverse;

    bool swap_bytes;    //!< Признак необходимости менять местами байты в принятых данных.
};

template<class Interface, class DataParser>
class EncoderSsi : public IEncoder, public EncoderSsiConfig {
public:

    EncoderSsi( DataParser _parser, Interface & _interface, EncoderSsiConfig::Config config )
        : EncoderSsiConfig(config), interface(_interface), parser(_parser) {}

    virtual ~EncoderSsi() = default;

    float getAngle() const override { return error_counter.counter ? NAN : encoder_angle; }
    float getTurn() const override { return error_counter.counter ? NAN : encoder_turn & turn_bit_mask; }
    std::pair<float, float> getPosition() const override;

//    unsigned short getErrorCode() const { return encoder_error_code; }
    float getErrorCode() const { return encoder_error_code; }

    bool isValidData() const { return not error_counter.counter; }
    bool isFailure() const { return error_counter.counter >= error_counter.max_error; }
    bool isDirectionError() const { return direction_error; }

    void process();

    uint32_t getRawData();

private:
    DataParser parser;
    Interface & interface;

    unsigned short encoder_turn = 0;
    float encoder_angle = NAN;      //только угол

    typename DataParser::EncoderData encoder_position;   //угол + обороты + ошибка

    unsigned short encoder_error_code = 0;
    bool direction_error = false; //!<Программно определяемая ошибка в данных энкодера, связанная со скачкообразным изменением показаний.

    DoubleBuffer< std::pair<float, float> > data;


};


} /* namespace detail */
} /* namespace driver */

template<class Interface, class DataParser>
inline void driver::detail::EncoderSsi<Interface, DataParser>::process() {

    //TODO: сперва нужно проверить есть ли у connection новые данные для нас, иначе он выдаст старые..
    uint32_t encoder_data = getRawData();

    typename DataParser::EncoderData next_position_data = parser.parseEncoderData( encoder_data );

    if( encoder_error_code = next_position_data.error ) {

        if( error_counter.counter < error_counter.max_error )
            error_counter.counter++;

    } else {

        encoder_angle = next_position_data.angle;
        encoder_turn = next_position_data.turn;

        //обработчки скачков показаний энкодера в оборотах
        direction_error = parser.getAngleDelta( next_position_data, encoder_position ) > max_angle_delta;

        encoder_position = next_position_data;

        if( error_counter.counter )
            --error_counter.counter;

        data.write( std::pair<float, float>( encoder_turn, encoder_angle ) );
    }

}

template<class Interface, class DataParser>
inline std::pair<float, float> driver::detail::EncoderSsi<Interface, DataParser>::getPosition() const {

    return error_counter.counter ? std::pair<float, float>( 0, NAN ) : data.read();

}

template<class Interface, class DataParser>
inline uint32_t driver::detail::EncoderSsi<Interface, DataParser>::getRawData() {

    if( swap_bytes ) {

        uint32_t raw_encoder_data = interface.get();

        uint32_t encoder_data = ( raw_encoder_data << 16 ) | ( (raw_encoder_data >> 16) & 0xFFFF );

        if( inverse ) encoder_data = ~encoder_data;

        encoder_data >>= shift_count;

        return encoder_data;

    } else {

        uint32_t encoder_data = interface.get();

        if( inverse ) encoder_data = ~encoder_data;

        return encoder_data;
    }

}

#endif /* SOURCE_DRIVER_ENCODERSSI_HPP_ */