/*
 * PrimaryEncoderSetup.cpp
 *
 *  Created on: 3 окт. 2019 г.
 *      Author: user
 */

#include "PrimaryEncoderSetup.hh"

#include "../../common/Codec.hpp"

#include "../../systemic/ValueLazyFast.hpp"
#include "../../systemic/Scheduler.hpp"
#include "../../systemic/Process.hpp"

#include "../../driver/SpiPortReader.hpp"
#include "../../driver/EncoderEncoderDataParser.hpp"
#include "../../driver/EncoderSsi.hpp"

typedef uint32_t SSI32_Frame;
const short ssi_frame_size = 32;

bool application::board::PrimaryEncoderSetup::input( Environment & env ) {

    return     grab( serial_port, env.rholder.getShared< peripheral::ISerialPort >( links.serial_port_id ) )
            && grab( de_pin, env.rholder.getShared< peripheral::IGpio >( links.de_pin_id ) )
            && grab( power_pin, env.rholder.getShared< peripheral::IGpio >( links.power_pin_id ) )
            && grab( cs21_pin, env.rholder.getShared< peripheral::IGpio >( links.cs21_id ) )
            && grab( cs22_pin, env.rholder.getShared< peripheral::IGpio >( links.cs22_id ) )
            ;

}

void application::board::PrimaryEncoderSetup::build( Environment & env ) {

    de_pin->write( config.de_active_level );
    power_pin->write( config.power_pin_active_level );

    cs21_pin->write( config.cs21_active_level );
    cs22_pin->write( config.cs22_active_level );

    typedef codec::GrayCodeDecoder DecoderType;
    typedef driver::EncoderDataParser<DecoderType>::ParserType EncoderDataParserType;

    DecoderType decoder;

    EncoderDataParserType * encoder_data_parser = memories::instance_object<EncoderDataParserType>(
            env.static_object_ma, decoder //codec::GrayCodeDecoder()
    );

    EncoderDataParserType::Setting parser_setting; {
        parser_setting.turn_bit_len = config.turn_bit_len,
                parser_setting.angle_bit_len = config.angle_bit_len,
                parser_setting.error_bit_len = config.error_bit_len;
    };

    encoder_data_parser->cofigure( parser_setting );

    SSI32_Frame default_frame = 0;

    driver::SpiPortReader<SSI32_Frame> * obj = memories::instance_object< driver::SpiPortReader<SSI32_Frame> >(
            env.static_object_ma, *serial_port, ssi_frame_size, default_frame );

    typedef driver::detail::EncoderSsi< driver::SpiPortReader<SSI32_Frame>, EncoderDataParserType> Encoder;

    driver::detail::EncoderSsiConfig::Config encoder_config;

    const short ssi_bit_len = config.turn_bit_len + config.angle_bit_len + config.error_bit_len;

    {
        encoder_config.num_error = 5;
        encoder_config.max_angle_delta = 6.28f;
        encoder_config.ssi_data_bit_len = ssi_bit_len;
        encoder_config.max_turn_bit = config.max_turn_bit_len;
        encoder_config.frame_size = ssi_frame_size;
        encoder_config.ssi_extra_bit = 0;
        encoder_config.shift_count = config.shift_bit_count;
        encoder_config.inverse = config.inverse;
        encoder_config.swap_bytes = true;
    }

    Encoder * encoder_driver = memories::instance_object<Encoder>(
            env.static_object_ma,
            *encoder_data_parser,
            *obj,
            encoder_config
    );

    systemic::ProcScheduler::ProcessParameter proc = {
        .priority = params.priority,
        .period = params.period, .phase = params.phase,
        .controlled = false
    };

    env.scheduler.registerProcess( params.processor_id, *memories::instance_object< systemic::Process<Encoder> >( env.static_object_ma, *encoder_driver ),
                                   proc );

    //todo Вынести
    //todo: Переписать!. clang не понимает typename =)

    env.rholder.share<driver::IEncoder>( *encoder_driver, links.primary_encoder_id );

    typedef systemic::detail::ValueLazyFast< bool, Encoder, &Encoder::isValidData> DataValidStatus;
    env.statuses.add( links.data_valid_id, memories::instance_object<DataValidStatus>( env.static_setup_ma, *encoder_driver ) );

    typedef systemic::detail::ValueLazyFast< bool, Encoder, &Encoder::isFailure> FailureStatus;
    env.statuses.add( links.failure_id, memories::instance_object<FailureStatus>( env.static_setup_ma, *encoder_driver ) );

    //Регистрация сигналов для демо версии:
    typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getAngle> EncoderAngle;
    env.signals.add( links.encoder_angle_sig_id, memories::instance_object<EncoderAngle>( env.static_setup_ma, *encoder_driver ) );

    typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getTurn> EncoderTurn;
    env.signals.add( links.encoder_turn_sig_id, memories::instance_object<EncoderTurn>( env.static_setup_ma, *encoder_driver ) );

    typedef systemic::detail::ValueLazyFast< float, Encoder, &Encoder::getErrorCode> EncoderErr;
    env.signals.add( links.encoder_err_sig_id, memories::instance_object<EncoderErr>( env.static_setup_ma, *encoder_driver ) );

}

application::board::PrimaryEncoderSetup::PrimaryEncoderSetup(
        const Links & links, const Setting & config, const configuration::ProcessConfig & proc) :
                links(links), config(config), params(proc)
{}