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

public class BrushDcMotor : MonoBehaviour {
	public double maxRpm = 3000;
    public double angularSpeedRPM;
	public double angularSpeedRPS; //radians per second

    [Header("[State]")]
    public double current = 0.0f;
    public double torque = 0.0f;
    public double LoadTorque = 0.0;

    [Header("[Editable variables]")]
    public double k_Friction = 7.71074154118413e-06; // N.m.s //b
    public double k_Torque = 1.0999964640773; // N.m/Amp
    public double k_BackEMF = 0.00112523063632681;// V/rad/sec
    public double rotorInertia = 5.5370674874546e-07;//kg.m^2
    public double Vin = 3.0;//V
    public double R = 16.6; //ohm
    public double L = 0.0009; //H
    private double previousSpeedDerivative;
    private double previousCurrentDerivative;

    public void Integrate(double dt) {
        //calc derivatives
        var currentDerivative = (Vin - current*R - k_BackEMF*angularSpeedRPS)/L;
        var speedDerivative =  (-k_Friction * angularSpeedRPS + k_Torque * current - LoadTorque) / rotorInertia;

        //trapezoidal integration
        angularSpeedRPS += (speedDerivative + previousSpeedDerivative) * 0.5 * dt;
        current += (currentDerivative + previousCurrentDerivative) * 0.5 * dt;

        angularSpeedRPM = MathEx.RadSToRpm * angularSpeedRPS;

        previousSpeedDerivative = speedDerivative;
        previousCurrentDerivative = currentDerivative;
    }

    void FixedUpdate()
    {
        double dt = Time.fixedDeltaTime;
        for (int i  = 0; i < 10; ++i)
        {
            Integrate(dt / 10.0);
        }
       
    }

    public void ResetState() {
        angularSpeedRPM = 0;
        angularSpeedRPS = 0;
        current = 0;
        torque = 0;
        LoadTorque = 0;
        previousSpeedDerivative = 0;
        previousCurrentDerivative = 0;
    }
}