using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using UnityEditor;
using System.Linq;
using System;

public class MotorPlot : MonoBehaviour {

    private Vector3[] plot_current;
    private Vector3[] plot_rps;
    private Vector3[] plot_power;
    private Vector3[] plot_in_power;
    private Vector3[] plot_eff;


    public Motor motor;

    const float plotHeight = 10.0f;
    const float plotWidth = plotHeight * 1.25f;

    Vector3[] CreatePlot(Plot plot) {
        Vector3[] result = new Vector3[plot.Samples.Count];
        for (int i = 0; i < plot.Samples.Count; ++i) {
            float x = (plotWidth / plot.Samples.Count) * i;
            result[i] = new Vector3(x, 0, (float)(plot.Samples[i] * plotHeight * plot.NormalizeValue));
        }
        return result;
    }

    public class Plot {
        public Plot(double _NormalizeValue) {
            NormalizeValue = _NormalizeValue;
        }
        public double NormalizeValue;
        public List<double> Samples = new List<double>();

        public double GetMin() {
            return Samples.Min();
        }

        public double GetMinNormalized() {
            return Samples.Min() * NormalizeValue;
        }

        public double GetMaxNormalized() {
            return Samples.Max() * NormalizeValue;
        }

        public double GetMax() {
            return Samples.Max();
        }
    }

    public class Plots {
        public Plot current = new Plot(1000.0 / 200.0);
        public Plot rpm = new Plot(1.0 / 24000.0);
        public Plot power = new Plot((1.0 * 1000.0f / 150.0));
        public Plot in_power = new Plot((1.0 * 1000.0f / 150.0));
        public Plot eff = new Plot(100.0 / 54.0);
    }

    private double k_torque_backup;
    private double k_friction_backup;
    private double k_backemf_backup;
    public int maxOptimizationIterations = 10000;
    public int integrationIterations = 1000;

    private static void fvec(double[] arg, double[] fi, object obj) {
        //errors:  eff_max, pow_max, rpm_max, cur_min, cur_max
        //k_Friction, k_Torque, k_BackEMF, rotorInertia

        var m = (MotorPlot)obj;
        m.motor.k_Friction = m.k_friction_backup + arg[0];
        m.motor.k_Torque = m.k_torque_backup + arg[1];
        m.motor.k_BackEMF = m.k_backemf_backup + arg[2];

        var plots = m.IntegratePlots();

        var eff_max_norm = plots.eff.GetMaxNormalized();
        var pow_max_norm = plots.power.GetMaxNormalized();
        var cur_max_norm = plots.current.GetMaxNormalized();

        var target_eff_max_norm = (52.0 / 100.0) * plots.eff.NormalizeValue;
        var target_pow_max_norm = (120.3 / 1000.0) * plots.power.NormalizeValue;
        var target_rpm_max_norm = 23050.0 * plots.rpm.NormalizeValue;
        var target_cur_min_norm = (17.0 / 1000.0) * plots.current.NormalizeValue;
        var target_cur_max_norm = (180.0 / 1000.0) * plots.current.NormalizeValue;


        var target_rpm_derivative = (0 - target_rpm_max_norm) / plotWidth;
        var iter_rpm_derivative = (plots.rpm.GetMinNormalized() - (plots.rpm.Samples[plots.rpm.Samples.Count / 2] * plots.rpm.NormalizeValue)) / (plotWidth / 2);

        var target_cur_derivative = (target_cur_max_norm - target_cur_min_norm) / plotWidth;
        var iter_cur_derivative = (plots.current.GetMaxNormalized() - (plots.current.Samples[plots.current.Samples.Count / 2] * plots.current.NormalizeValue)) / (plotWidth / 2);

        var err_eff = target_eff_max_norm - eff_max_norm;
        var err_pow = target_pow_max_norm - pow_max_norm;
        var err_rpm_deriv = target_rpm_derivative - iter_rpm_derivative;
        var err_cur = target_cur_derivative - iter_cur_derivative;
        var err_cur_max = target_cur_max_norm - cur_max_norm;

        fi[0] = err_pow * err_pow;
        fi[1] = err_rpm_deriv * err_rpm_deriv;
        fi[2] = err_cur * err_cur;
        fi[3] = err_cur_max * err_cur_max;
        fi[4] = err_eff * err_eff;
    }

    public void Optimize() {
        const double optStep = 0.0000001;
        k_torque_backup = motor.k_Torque;
        k_friction_backup = motor.k_Friction;
        k_backemf_backup = motor.k_BackEMF;

        double[] Params = new double[3] { 0, 0, 0 };

        alglib.minlmstate state;
        alglib.minlmreport rep;

        try {
            //params count, errors count, params
            alglib.minlmcreatev(Params.Length, 6, Params, optStep, out state);
        } catch (Exception ex) {
            Debug.Log("Failed to optimize, bad params! " + ex.Message);
            return;
        }


        alglib.minlmsetcond(state, 0.00000000001, 0.00000000001, 0.00000000001, maxOptimizationIterations);
        //alglib.minlmsetbc(state, FunctionParametersMin, FunctionParametersMax);
        alglib.minlmoptimize(state, fvec, null, this);

        alglib.minlmresults(state, out Params, out rep);


        motor.k_Friction = k_friction_backup + Params[0];
        motor.k_Torque = k_torque_backup + Params[1];
        motor.k_BackEMF = k_backemf_backup + Params[2];

        Debug.Log(rep.iterationscount);
        IntegratePlotsVisual();
    }



    public void DrawUnboundPlots() {
        Plots result = new Plots();
        motor.ResetState();

        plot_eff = null;
        plot_in_power = null;
        plot_power = null;
        plot_rps = null;
        plot_current = null;

        var oldVoltage = motor.Vin;

        for (int i = 0; i < integrationIterations; ++i) {

            if (i > (integrationIterations / 2)) {
                motor.Vin = 0;
            }

            result.current.Samples.Add(motor.current);
            result.rpm.Samples.Add(motor.angularSpeedRPM);
            motor.Integrate(motor.integrationStep);
        }
        motor.Vin = oldVoltage;
        plot_rps = CreatePlot(result.rpm);
        plot_current = CreatePlot(result.current);
        SceneView.RepaintAll();
    }

    public Plots IntegratePlots() {
        Plots result = new Plots();
        motor.ResetState();

        motor.LoadTorque = 0;
        double loadStep = 0.04185 * motor.integrationStep;

        //warm up
        for (int i = 0; i < (int)(1.0f / motor.integrationStep) * 100; ++i) {
            motor.Integrate(motor.integrationStep);
        }

        int iteration = 0;

        //add load torque until motor stall
        while (motor.angularSpeedRPM > 0.0) {
            ++iteration;
            motor.LoadTorque = loadStep * iteration;

            // double power = (k_BackEMF / k_Torque) * angularSpeedRPS * (torque - torqueOffset);
            double power = motor.angularSpeedRPS * motor.LoadTorque / 1000.0;
            double inPower = motor.current * motor.Vin;
            double eff = power / inPower;

            result.current.Samples.Add(motor.current);
            result.rpm.Samples.Add(motor.angularSpeedRPM);
            result.eff.Samples.Add(eff);
            result.power.Samples.Add(power);
            result.in_power.Samples.Add(inPower);

            for (int m = 0; m < 10; ++m) {
                motor.Integrate(motor.integrationStep);
            }

        }
        Debug.Log("Load torque = " + motor.LoadTorque);
        return result;
    }

    public void IntegratePlotsVisual() {
        var plots = IntegratePlots();

        //convert values to plots
        plot_current = CreatePlot(plots.current);
        plot_rps = CreatePlot(plots.rpm);
        plot_power = CreatePlot(plots.power);
        plot_in_power = CreatePlot(plots.in_power);
        plot_eff = CreatePlot(plots.eff);

        SceneView.RepaintAll();
    }

    void OnDrawGizmos() {
        Handles.matrix = Matrix4x4.identity;
        Handles.color = Color.red;
        if (plot_current != null) {
            Handles.DrawPolyLine(plot_current);
        }

        Handles.color = Color.blue;
        if (plot_rps != null) {
            Handles.DrawPolyLine(plot_rps);
        }

        Handles.color = Color.yellow;
        if (plot_power != null) {
            Handles.DrawPolyLine(plot_power);
        }

        Handles.color = Color.green;
        if (plot_eff != null) {
            Handles.DrawPolyLine(plot_eff);
        }

        Handles.color = Color.gray;
        if (plot_in_power != null) {
            Handles.DrawPolyLine(plot_in_power);
        }

        Handles.color = Color.black;
        //draw borders
        Vector3 upOffset = Vector3.forward * plotHeight;
        Vector3 rightOffset = Vector3.right * plotWidth;

        Handles.DrawLine(Vector3.zero, upOffset);
        Handles.DrawLine(Vector3.zero, rightOffset);
        Handles.DrawLine(upOffset, upOffset + rightOffset);
        Handles.DrawLine(rightOffset, upOffset + rightOffset);
    }
}