/*
 * NullEstimation.cpp
 *
 *  Created on: 23 нояб. 2016 г.
 *      Author: titov
 */

#include "NullEstimation.hh"
#include <cstring>

technological::function::NullEstimation::NullEstimation( ResourceKeeper<driver::IInverter> & rh_inverter, ResourceKeeper<driver::IBrake> & rh_brake,
                                                         systemic::ISignal & position,
                                                         ResourceKeeper<std::pmr::monotonic_buffer_resource> & rh_memory ) :
		        inverter(rh_inverter), brake(rh_brake), buffer(rh_memory), setting(),
		        is_run(false), position(position), output( 0, 0 ),
		        proc_ctrl_observe(nullptr), proc_ctrl_collection(nullptr),
		        observe_and_calculation(*this), data_collection(*this) {}

bool technological::function::NullEstimation::getResult( char * value,
                                                         std::size_t size ) const {

    float result = estimator.get_result();

    if( sizeof(result) != size )
        return false;

    std::memcpy( value, &result, size );

    return true;

}

void technological::function::NullEstimation::setProcessController_Observe(
        systemic::IProcessControl * proc_ctrl ) {

    proc_ctrl_observe = proc_ctrl;

}

void technological::function::NullEstimation::setProcessController_DataCollection(
        systemic::IProcessControl * proc_ctrl ) {

    proc_ctrl_collection = proc_ctrl;

}

bool technological::function::NullEstimation::validate( Input & input ) {

    return ( input.amp_modulation > 0.0f ) && ( input.amp_modulation < 1.0f )
            && ( input.speed > 0.0f ) && ( input.Nc > 0 ) && ( input.np > 0 ) && ( input.rise_to_hold > 0 );

}

bool technological::function::NullEstimation::run( const char * value, std::size_t size ) {

    if( is_run )
        return false;

    if( not proc_ctrl_observe or not proc_ctrl_collection )
        return false;

    if( sizeof(Input) != size )
        return false;

    Input input;
    std::memcpy( &input, value, size );

    if( not validate(input) )
        return false;

    setting.Np = input.np;

    ScopeLock< Locable<driver::IInverter>, Locable<driver::IBrake>, Locable<std::pmr::monotonic_buffer_resource> > lock( inverter, brake, buffer );

    if( lock
            and estimator.setBuffer( buffer.operator->() )
            and estimator.configure( setting )
            and estimator.start( input.speed, input.Nc, input.rise_to_hold ) ) {

        output.d = input.amp_modulation;
        output.q = 0.0f;

        brake->disengage();
        inverter->enPulse();

        proc_ctrl_observe->enable();
        proc_ctrl_collection->enable();

        lock.freeze();

        is_run = true;

    }

    return is_run;
}

void technological::function::NullEstimation::stop() {

    if( is_run ) {

        proc_ctrl_observe->disable();
        proc_ctrl_collection->disable();

        output = control::RotatingVector(0.0f, 0.0f);

		brake->engage();
		brake.unlock();

		inverter->disPulse();
		inverter.unlock();

		estimator.stop();
		estimator.resetBuffer();

		buffer->release();
		buffer.unlock();

		is_run = false;

	}
}

short technological::function::NullEstimation::getState() const {

	if( not(is_run) )                                                       return DISABLE;
	else if( estimator.isCalcReady() )                                      return FINISHED;
	else if( estimator.isDataCollection() || estimator.isDataAnalysis() )   return EXECUTE;
	else if( estimator.isDataFailure() )                                    return FAILURE;
	else                                                                    return -128;

}

technological::function::NullEstimation::DataCollectionProcess::DataCollectionProcess(
        NullEstimation & outer ) : outer(outer) {}

void technological::function::NullEstimation::DataCollectionProcess::setSampleTime(
        float ts_in_second ) {

    outer.setting.Ts = ts_in_second;

}

void technological::function::NullEstimation::DataCollectionProcess::process() {

    float iota;
    outer.estimator.execute( outer.position, iota );

    control::NaturalCoordinate switch_time =
            control::function::SpaceVectorPwm::generate( control::itf_park( outer.output, iota ) );

    outer.inverter->pwm( switch_time.u, switch_time.v, switch_time.w );

}

void technological::function::NullEstimation::ObserveAndCalculationProcess::setSampleTime(
        float ts_in_second ) {}

technological::function::NullEstimation::ObserveAndCalculationProcess::ObserveAndCalculationProcess(
        NullEstimation & outer ) : outer(outer) {}

void technological::function::NullEstimation::ObserveAndCalculationProcess::process() {

    if( outer.estimator.isDataFailure() || !outer.estimator.isDataCollection() ) {
        outer.brake->disengage();

        outer.inverter->disPulse();

        outer.output = control::RotatingVector(0.0f, 0.0f);
    }

    if( outer.estimator.isDataReady() )
        outer.estimator.process();

}