/*
 * BinaryEncoder.hpp
 *
 *  Created on: 23 июн. 2023 г.
 *      Author: titov
 */

#ifndef UMLIBRARY_DRIVER_BINARYENCODER_HPP_
#define UMLIBRARY_DRIVER_BINARYENCODER_HPP_


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

#include "IEncoder.hh"
#include "../communication/format/BinaryDataPublisher.hh"

#include "../common/DoubleBuffer.hpp"
#include "../communication/format/BinaryHelpers.hh"

#include <atomic>

#include "../math/math_inc.hh"
#include <cstring>

namespace driver { namespace detail {

struct BinaryEncoderInterface : public IEncoder, public communication::IBinaryDataSubscriber {};

//!Базовый класс драйвера энкодера, обновляющего свои значения в момент обновления.
template<typename FrameType>
class BinaryEncoderBase : public BinaryEncoderInterface {
public:
    typedef communication::format::bits::bitsize bitsize;

    BinaryEncoderBase( bitsize angle_size, bitsize turn_size );

    //!Функция возращает номер оборота.
    Turn getTurn() const;

    //!Функция возращает угол в радианах от 0 до 2*pi или NaN.
    Angle getAngle() const;

    //!Метод позволяет получить консистентные данные позиции энкодера в формате (оборот, угол).
    std::pair<Turn, Angle> getPosition() const;

protected:
    typedef FrameType frame;

    mutable std::atomic<FrameType> point;

private:
    const bitsize turn_offset;
    const frame turn_mask;
    const frame angle_mask;

    const Angle angle_cost;

};

template<typename FrameType>
inline driver::detail::BinaryEncoderBase<FrameType>::BinaryEncoderBase( bitsize angle_size, bitsize turn_size ) :
    turn_offset(angle_size),
    turn_mask( ( 1ull << turn_size ) - 1ull ),
    angle_mask( ( 1ull << angle_size ) - 1ull),
    angle_cost( math::constants::pi2 / std::pow(2.0f, float(angle_size) ) ), point() {

    point.store(0);

}

template<typename FrameType>
inline driver::IEncoder::Turn driver::detail::BinaryEncoderBase<FrameType>::getTurn() const {

    frame current_frame = point.load();

    Turn turn = ( current_frame >> turn_offset ) & turn_mask;

    return turn;

}

template<typename FrameType>
inline driver::IEncoder::Angle driver::detail::BinaryEncoderBase<FrameType>::getAngle() const {

    frame current_frame = point.load();

    Angle angle = ( current_frame & angle_mask ) * angle_cost;

    return angle;

}

template<typename FrameType>
std::pair<
    driver::IEncoder::Turn,
    driver::IEncoder::Angle
> driver::detail::BinaryEncoderBase<FrameType>::getPosition() const {

    frame current_frame = point.load();

    Turn turn = ( current_frame >> turn_offset ) & turn_mask;
    Angle angle = ( current_frame & angle_mask ) * angle_cost;

    return std::pair<Turn, Angle>( turn, angle );

}


//!Драйвер энкодера в бинарном формате.
template<typename FrameType>
class BinaryEncoder : public BinaryEncoderBase<FrameType> {
public:
    typedef communication::format::bits::bitsize bitsize;

    BinaryEncoder( bitsize angle_size, bitsize turn_size );

    /*!
     * data - угол и оборот в бинароном виде, угол со смещением 0.
     */
    void read( const void * data, std::size_t size ) override;

};

template<typename FrameType>
BinaryEncoder<FrameType>::BinaryEncoder( bitsize angle_size, bitsize turn_size ) :
    BinaryEncoderBase<FrameType>(angle_size, turn_size) {}

template<typename FrameType>
void driver::detail::BinaryEncoder<FrameType>::read( const void * data,
                                                     std::size_t size ) {

    typename BinaryEncoderBase<FrameType>::frame new_frame;
    std::memcpy( &new_frame, data, sizeof(new_frame) );
    BinaryEncoderBase<FrameType>::point.store( new_frame );

}

//!Драйвер энкодера в бинарном формате, включает инверсию входных данных для изменения направления вращения.
template<typename FrameType>
class BinaryEncoderInverted : public BinaryEncoderBase<FrameType> {
public:
    typedef communication::format::bits::bitsize bitsize;

    BinaryEncoderInverted( bitsize angle_size, bitsize turn_size );

    void read( const void * data, std::size_t size ) override;

};

template<typename FrameType>
BinaryEncoderInverted<FrameType>::BinaryEncoderInverted( bitsize angle_size, bitsize turn_size ) :
    BinaryEncoderBase<FrameType>(angle_size, turn_size) {}

template<typename FrameType>
void driver::detail::BinaryEncoderInverted<FrameType>::read( const void * data,
                                                             std::size_t size ) {

    typename BinaryEncoderBase<FrameType>::frame  new_frame;
    std::memcpy( &new_frame, data, sizeof(new_frame) );
    BinaryEncoderBase<FrameType>::point.store( ~new_frame );

}

//!Драйвер энкодера в формате кода грей.
template<typename FrameType>
class BinaryEncoderGray : public BinaryEncoderBase<FrameType> {
public:
    typedef communication::format::bits::bitsize bitsize;

    BinaryEncoderGray( bitsize angle_size, bitsize turn_size );

    void read( const void * data, std::size_t size ) override;

};

//!Драйвер энкодера в формате кода грей, включает инверсию входных данных для изменения направления вращения.
template<typename FrameType>
class BinaryEncoderGrayInverted : BinaryEncoderBase<FrameType> {
public:
    typedef communication::format::bits::bitsize bitsize;

    BinaryEncoderGrayInverted( bitsize angle_size, bitsize turn_size );

    void read( const void * data, std::size_t size ) override;

};

}}

#endif /* UMLIBRARY_DRIVER_BINARYENCODER_HPP_ */