/*
 * EstimateInductance.cpp
 *
 *  Created on: 23 θών. 2021 γ.
 *      Author: titov
 */

#include "EstimateInductance.hh"

technological::commissioning::EstimateInductance::EstimateInductance(
        std::pmr::memory_resource * buffer) : points(buffer) {}

void technological::commissioning::EstimateInductance::setup(
        units::ElectricalAngle angle, control::Value current_limit,
        control::Value voltage_limit, control::Value voltage_step,
        units::Quantity impulse_width, units::Quantity relax_width,
        units::Quantity experiment_quantity,
        control::Value voltage_min ) {

    points.resize(experiment_quantity, Point(control::StandingVector(0, 0), control::StandingVector(0,0 )) );

    {
        this->angle = angle;
        this->current_limit = current_limit;
        this->voltage_limit = voltage_limit;
        this->voltage_step = voltage_step;
        this->impulse_width = impulse_width;
        this->relax_width = relax_width;
        this->experiment_quantity = experiment_quantity;
        this->voltage_min = voltage_min;
    }

    impulse_cycle_number = 0;
    relax_cycle_number = 0;
    experiment_number = 0;
    voltage_demand = 0;

}

control::StandingVector technological::commissioning::EstimateInductance::execute(
        control::StandingVector current ) {

    if( relax_cycle_number == relax_width ) {

        control::StandingVector voltage =
                control::itf_park(control::RotatingVector( 0.0f, voltage_demand ), angle);

        if( impulse_cycle_number == impulse_width ) {

            if( experiment_number < experiment_quantity ) {

                if( current.len() >= target_current_on_experiment(experiment_number)
                        or voltage_demand == voltage_limit ) {

                    points[experiment_number++] =
                            Point( voltage, current );

                } else {

                    const float voltage_next = voltage_demand + voltage_step;
                    voltage_demand = voltage_next < voltage_limit ? voltage_next : voltage_limit;

                }

                impulse_cycle_number = 0;
                relax_cycle_number = 0;

            } else
                voltage_demand = 0;

        } else
            ++impulse_cycle_number;

        return voltage;

    } else
        ++relax_cycle_number;

    return control::StandingVector(0, 0);



}

bool technological::commissioning::EstimateInductance::done() const {

    return experiment_number == experiment_quantity;

}

units::Inductance technological::commissioning::EstimateInductance::estimate(
        units::Quantity pwm_delay, units::Resistance resistance,
        units::Time cycle_time ) const {

    double square_current = 0;
    double voltage_pulse = 0;

    for( Points::const_iterator iter = points.begin(), end = points.end(); iter != end; ++iter ) {

        double voltage_len = iter->first.len();
        double current_len = iter->second.len();
        double current_sqrlen = iter->second.sqrlen();

        if( voltage_len >= voltage_min ) {
            square_current += current_sqrlen;
            voltage_pulse += ( voltage_len - current_len * resistance * 0.5 )
                    * (cycle_time * ( impulse_width - pwm_delay )) * current_len;
        }
    }

    return voltage_pulse / square_current;

}

control::Value technological::commissioning::EstimateInductance::target_current_on_experiment(
        units::Quantity experiment_number ) {

    return ( current_limit * experiment_number ) / experiment_quantity;

}