/*
 * EncoderDataParser.hpp
 *
 *  Created on: 21 мар. 2019 г.
 *      Author: krugliy
 */

#ifndef SOURCE_DRIVER_ENCODERDATAPARSER_HPP_
#define SOURCE_DRIVER_ENCODERDATAPARSER_HPP_


#include <stdint.h>
#include "../math/math_inc.hh"

namespace driver {

//!
template<class Decoder>
class EncoderDataParser {
public:
    typedef EncoderDataParser<Decoder> ParserType;
    typedef long CodeType;

    struct Setting {
        uint16_t angle_bit_len;
        uint16_t turn_bit_len;
        uint16_t error_bit_len;
    };

    struct EncoderData {
        float angle;
        unsigned long turn;
        unsigned short error;
    };

    EncoderDataParser(Decoder _decoder)
        : decoder(_decoder) {}

    EncoderDataParser(const EncoderDataParser &) = default;

    float getAngleDelta( const EncoderData & p1, const EncoderData & p2 ) {

        long signed_turn1 = p1.turn << ((32ul - turn_signbit_offset)) >> turn_signbit_offset;
        long signed_turn2 = p2.turn << ((32ul - turn_signbit_offset)) >> turn_signbit_offset;

        return std::abs( math::constants::pi2*(signed_turn1 - signed_turn2) + (p1.angle - p2.angle) );

    }

    EncoderData parseEncoderData( unsigned long raw_encoder_data ) {

        position_code = decoder((raw_encoder_data & position_code_mask) >> position_bit_offset, position_resolution );

        return {
            .angle = angle_code_to_rad * ( (position_code & angle_code_mask) >> angle_offset ),
            .turn = ( position_code & turn_code_mask ) >> turn_offset,
            .error = static_cast<unsigned short>(( raw_encoder_data & error_code_mask ) >> error_offset)
        };

    }

    bool cofigure(Setting & set) {
        bool result = true;

        if( set.angle_bit_len == 0 ) result = false;

        if( result ) {

            //применять на raw_encoder_data
            position_resolution = set.angle_bit_len + set.turn_bit_len;
            position_bit_offset = set.error_bit_len;
            position_code_mask = ((1ul << position_resolution) - 1) << position_bit_offset;

            //применять на raw_encoder_data
            error_offset = 0;
            error_code_mask = ((1ul << set.error_bit_len) - 1) << error_offset;

            //применять на position_code
            angle_offset = 0;
            angle_code_mask = ((1ul << set.angle_bit_len) - 1) << angle_offset;

            //применять на position_code
            turn_offset = set.angle_bit_len;
            turn_code_mask = ((1ul << set.turn_bit_len) - 1) << turn_offset;

            //TODO следить за типом!
            turn_signbit_offset = set.turn_bit_len;

            angle_code_to_rad = math::constants::pi2 / (1ul << set.angle_bit_len);

        }

        return result;
    }

private:
    Decoder decoder;
    volatile unsigned long position_code;

    unsigned long position_resolution;
    unsigned long position_bit_offset;
    unsigned long position_code_mask;

    unsigned long angle_code_mask;
    unsigned long angle_offset;

    unsigned long turn_code_mask;
    unsigned long turn_offset;

    unsigned long error_code_mask;
    unsigned long error_offset;

    unsigned short turn_signbit_offset;      //!<Позиция старшего бита кода позиции.

    float angle_code_to_rad = NAN;

};

}


#endif /* SOURCE_DRIVER_ENCODERPOSITIONDATAPARSER_HPP_ */