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

#include "EstimateResistance.hh"

#include <algorithm>
#include <numeric>

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

void technological::commissioning::EstimateResistance::setup(
        units::ElectricalAngle angle, control::Value current_limit,
        control::Value voltage_limit, control::Value voltage_step,
        units::Quantity hold_quantity, units::Quantity experiment_quantity) {

    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->hold_quantity = hold_quantity;
        this->experiment_quantity = experiment_quantity;
    }

    hold_cycle_number = 0;
    experiment_number = 0;
    voltage_demand = 0;

}

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

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

    if( hold_cycle_number == hold_quantity ) {
        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 );

            }

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

            hold_cycle_number = 0;

        } else
            voltage_demand = 0;

    } else
        ++hold_cycle_number;

    return voltage;

}

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

    return experiment_number == experiment_quantity;

}

units::Resistance technological::commissioning::EstimateResistance::estimate() const {

    double square_current = 0;
    double power = 0;

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

        square_current += iter->second.sqrlen();
        power += iter->second.len() * iter->first.len();

    }

    return power / square_current;
}

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

    return ( current_limit * experiment_number ) / experiment_quantity;

}