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

public enum MotorPlotVariable {
    Current,
    AnglarSpeed,
    OutPower,
    InPower,
    Efficiency,
    Torque,
    Voltage,
    BackEMF
}

public enum AngularSpeedUnits {
    RevPS,
    RadPS,
    RevPM
}


public enum MotorPlotConstrainType {
    GlobalMin,
    GlobalMax,
    ValueAtX,
    DerivAtX,
    GlobalDeriv
}

[Serializable]
public class MotorPlotConstrain {
    public MotorPlotConstrainType type;
    public double x = 0;
}
[Serializable]
public class Plot
{
    public Color color = Color.blue;
    public MotorPlotVariable variable;
    public double minValue;
    public double maxValue;

    [HideInInspector]
    public List<double> SamplesX = new List<double>();
    [HideInInspector]
    public List<double> SamplesY = new List<double>();

    public double GetMinValue() {
        return SamplesY.Min();
    }

    public double GetMaxValue() {
        return SamplesY.Max();
    }

    public double NormalizeValue(double value) {
        return (value - minValue) / (maxValue - minValue);
    }

    public double GlobalValue(double normalized_value) {
        return normalized_value * (maxValue - minValue) + maxValue;
    }

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

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

public class MotorPlot : MonoBehaviour {
    public MotorPlotConstrain[] constrains;
    public Plot[] plots;

    public BrushDcMotor motor;
    public double integrationStep = 0.00001;
    public AngularSpeedUnits angularSpeedUnits = AngularSpeedUnits.RevPM;

    public float plotHeight = 10.0f;
    public float plotWidth = 12.5f;
    public int pointsSkip = 0;

    public float plotMinX = 0;
    public float plotMaxX = 1;

    public MotorPlotVariable plotXVariable = MotorPlotVariable.Current;

    private List<Vector3[]> plots_visual;

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

    


    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;

        //calculate errors
        for (int i = 0; i < m.constrains.Length; ++i) {
            var c = m.constrains[i];
            fi[i] = 0;
            // if( c.type == MotorPlotConstrainType.
            //fi[i] = 
        }


        /* 
         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) / m.plotWidth;
         var iter_rpm_derivative = (plots.rpm.GetMinNormalized() - (plots.rpm.Samples[plots.rpm.Samples.Count / 2] * plots.rpm.NormalizeValue)) / (m.plotWidth / 2);

         var target_cur_derivative = (target_cur_max_norm - target_cur_min_norm) / m.plotWidth;
         var iter_cur_derivative = (plots.current.GetMaxNormalized() - (plots.current.Samples[plots.current.Samples.Count / 2] * plots.current.NormalizeValue)) / (m.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, constrains.Length, 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.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(integrationStep);
        }
        motor.Vin = oldVoltage;
        plot_rps = CreatePlot(result.rpm);
        plot_current = CreatePlot(result.current);
        SceneView.RepaintAll();*/
    }

    public double GetVariable(MotorPlotVariable var) {
        switch (var) {
            case MotorPlotVariable.AnglarSpeed:
                if (angularSpeedUnits == AngularSpeedUnits.RevPM) {
                    return motor.angularSpeedRPM;
                } else if (angularSpeedUnits == AngularSpeedUnits.RadPS) {
                    return motor.angularSpeedRPS;
                } else {
                    throw new Exception("Unit conversion is not implemented");
                }
            case MotorPlotVariable.Current:
                return motor.current;
            case MotorPlotVariable.Torque:
                return motor.LoadTorque;
            case MotorPlotVariable.OutPower:
                return motor.angularSpeedRPS * motor.LoadTorque;//in Watts
            case MotorPlotVariable.InPower:
                return motor.current * motor.Vin;
            case MotorPlotVariable.Efficiency:
                return GetVariable(MotorPlotVariable.OutPower) / GetVariable(MotorPlotVariable.InPower);

        }
        return 0;
    }

    public double loadStepMul = 0.04185;
    public bool IntegratePlots() {
        if (plots == null || plots.Length == 0) {
            Debug.LogWarning("No plots to integrate!");
            return false;
        }



        motor.ResetState();
        motor.LoadTorque = 0;
        double loadStep = loadStepMul * integrationStep;

        foreach (var p in plots) {
            p.SamplesX.Clear();
            p.SamplesY.Clear();
        }

        //warm up
        for (int i = 0; i < (int)(1.0f / integrationStep) * 100; ++i) {
            motor.Integrate(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;*/

            if (iteration % (pointsSkip + 1) == 0) {
                var xValue = GetVariable(plotXVariable);
                foreach (var p in plots) {
                    p.SamplesY.Add(GetVariable(p.variable));
                    p.SamplesX.Add(xValue);
                }
            }

            for (int m = 0; m < 10; ++m) {
                motor.Integrate(integrationStep);
            }
        }
        Debug.Log("Load torque = " + motor.LoadTorque);

        Debug.Log(String.Format("{0,-25}{1,-25}{2,-25}{3,-25}{4,-25}", "Plot name", "Range min", "Range max", "Val min","Val max"));
        //print plots
        foreach (var p in plots)
        {
            var color = p.color.ToHex();
            Debug.Log(String.Format("<color={5}>{0,-25}</color>{1,-25}{2,-25}{3,-25}{4,-25}", p.variable.ToString(), p.minValue, p.maxValue, p.GetMinValue(), p.GetMaxValue(), color));
        }
        return true;
    }

    public void IntegratePlotsVisual() {
        if (!IntegratePlots())
        {
            return;
        }
        if (plots_visual == null) {
            plots_visual = new List<Vector3[]>();
        }
        plots_visual.Clear();
        foreach (var p in plots) {
            plots_visual.Add(CreatePlot(p));
        }
        SceneView.RepaintAll();
    }

    void OnDrawGizmos() {
        Handles.matrix = transform.localToWorldMatrix;

        if (plots_visual != null && plots != null && plots_visual.Count == plots.Length)
        {
            for (int i = 0; i < plots.Length; ++i)
            {
                var v = plots_visual[i];
                var p = plots[i];

                if (p != null && v != null) {
                    Handles.color = p.color;
                    Handles.DrawPolyLine(v);
                }
            }
        }


        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);

        Handles.matrix = Matrix4x4.identity;
    }
}