/*
 * BinaryEncoderSync.cpp
 *
 *  Created on: 27 ÿíâ. 2022 ã.
 *      Author: titov
 */

#include "BinaryEncoderSync.hh"

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

void driver::detail::BinaryEncoderSync::read( const void * data, std::size_t size ) {

    peripheral::nanotimer::stamp timestamp = (*peripheral::nanotimer::point)();

    frame raw;
    std::memcpy( &raw, data, sizeof(frame) );

    Points::Point new_point;
    new_point.turn = turn.get(raw);
    new_point.angle = angle_cost * angle.get(raw);
    new_point.timestamp = timestamp;

    Points current = points.load();

    current.second = current.first;
    current.first = new_point;

    points.store( current );

    handled.clear();

}

void driver::detail::BinaryEncoderSync::process() {

    peripheral::nanotimer::stamp timestamp = (*peripheral::nanotimer::point)();

    if( not handled.test_and_set() )
        sample.write( per_sample( timestamp ) );
    else
        sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );

}

std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::detail::BinaryEncoderSync::getPosition() const {

    return sample.read();

}

driver::detail::BinaryEncoderSync::BinaryEncoderSync( bitsize angle_offset, bitsize angle_size,
                                                      bitsize turn_offset, bitsize turn_size ) :
                                                              angle(angle_offset, angle_size), turn(turn_offset, turn_size),
                                                              angle_cost( math::constants::pi2 / std::pow(2.0f, float(angle_size)) ) {
    handled.clear();

    handled.clear();

    sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );
    sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );

    Points::Point default_point;
    default_point.turn = NAN;
    default_point.angle = NAN;
    default_point.timestamp = 0;

    Points default_points;
    default_points.first = default_point;
    default_points.second = default_point;

    points.store( default_points );

}

void driver::detail::BinaryEncoderSync::setSampleTime( float ts_in_second ) {

    cycle_time = ( ts_in_second * 1e9f );

}

driver::IEncoder::Turn driver::detail::BinaryEncoderSync::getTurn() const {

    return sample.read().first;

}

driver::IEncoder::Angle driver::detail::BinaryEncoderSync::getAngle() const {

    return sample.read().second;

}

std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::detail::BinaryEncoderSync::per_sample( peripheral::nanotimer::stamp timestamp ) {

    Points current = points.load();

    Points::Point & point_1 = current.first;
    Points::Point & point_2 = current.second;

    using namespace std;

    if( isfinite(current.first.turn) and isfinite(current.second.turn) ) {

        driver::IEncoder::Angle angle_increment =
                in_turn( delta( std::pair<Turn, Angle>(point_1.turn, point_1.angle), std::pair<Turn, Angle>(point_2.turn, point_2.angle) ),
                         point_2.turn );

        float reduced_time = float( (*peripheral::nanotimer::delta)( timestamp, point_2.timestamp ) ) /
                float( (*peripheral::nanotimer::delta)( point_1.timestamp, point_2.timestamp ) );

        point_2.angle += angle_increment * reduced_time;

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

    } else

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

}

driver::detail::BinaryEncoderSync::frame driver::detail::BinaryEncoderSync::Bitfield::get( frame raw ) {

    return ( raw >> offset ) & mask;

}

driver::detail::BinaryEncoderSync::Bitfield::Bitfield( bitsize new_offset,
                                                       bitsize new_size ) : offset( new_offset ), mask((1ul << new_size) - 1) {}