var     V0      = 12.0,
        Omega0  = 1371.83,
        I0      = 0.0444,
        R       = 9.07,
        K       = 0.842e-2,
        Jmot    = 0.541e-7;

var     Rho     = 67.49,
        Eta     = 0.75,
        Jgear   = 0.15e-8;

var     DeltaTheta,
        Nenc    = 256.0;

var     Larm    = 0.1,
        Marm    = 0.1,
        Mweight = 0.1;

var     G       = 9.8,
        DeltaT  = 0.001,
        SimDT   = 0.0001;

var     Omega   = 0.0,
        Theta   = 0.0, 
        Alpha   = 0.0,
        Time    = 0.0;

function  set_motor (form_id, mot_name)
{
    var  forms = document.getElementById(form_id);

    if (mot_name == "118578") {
        forms.V0.value     = 12.0;
        forms.Omega0.value = 1298.52;
        forms.I0.value     = 0.0446;
        forms.R.value      = 31.4;
        forms.K.value      = 0.818e-2;
        forms.Jmot.value   = 0.292e-7;
    }
    else if (mot_name == "118637") {
        forms.V0.value     = 12.0;
        forms.Omega0.value = 1371.83;
        forms.I0.value     = 0.0444;
        forms.R.value      = 9.07;
        forms.K.value      = 0.842e-2;
        forms.Jmot.value   = 0.541e-7;
    }
}

function  motor (V)
{
    var  I, Tmot, Tload, Jload;

    if (isNaN(Omega)) Omega = 0.0;
    I = (V - K * Omega) / R;
    Tmot  = K * I - (K * I0 / Omega0) * Omega;
    Tload = Mweight * Larm * G * Math.sin(Alpha);
    Jload = Marm * Larm * Larm / 3.0 + Mweight * Larm * Larm;

    return  (Tmot - Tload/(Eta*Rho)) / (Jmot + Jgear + Jload/(Rho*Rho));
}

function enc ()
{
    return  Math.floor(Theta / DeltaTheta);
}

function  sim_step (V) 
{
    var  Omega_old;

    Omega_old = Omega;
    Omega += motor(V) * SimDT;
    Theta += (Omega_old + Omega) / 2.0 * SimDT;
    Alpha = Theta / Rho;
    Time += SimDT;
}

function  sim_loop (V, Dt)
{
    var  Tend;

    Tend = Time + Dt;
    while (Time < Tend)
        sim_step(V);
}

var  Vmp = 12.0;
var  Kp, Ki, Kd;
var  Vi = 0.0;
var  old_th = 0.0;

function cont_init ()
{
    Omega  = 0.0;
    Theta  = 0.0;
    Alpha  = 0.0;
    Time   = 0.0;
    Vi     = 0.0;
    old_th = 0.0;
    DeltaTheta = (2.0 * Math.PI) / (Nenc * 4);
}

function cont_step (Theta_goal)
{
    var  th, e, Vp, Vd, V;

    th = enc() * DeltaTheta;
    e = Theta_goal - th;
    Vp = Kp * e;
    Vi += Ki * e * DeltaT;
    if (Vi > Vmp) Vi = Vmp; else if (Vi < -Vmp) Vi = -Vmp;
    Vd = - Kd * (th - old_th) / DeltaT;
    V = Vp + Vi + Vd;
    if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
    old_th = th;

    sim_loop(V, DeltaT);
}

var  DeltaTdraw = 0.005;

function  cont_loop (Agoal, tend)
{
    var  td;

    gplot(Time, Alpha);
    td = Time;
    while (Time < tend) {
        cont_step(Agoal*Rho);
        if (Time - td >= DeltaTdraw) {
            gplot(Time, Alpha);
	    td = Time;
	}
    }
}

var  Tend1, Tend2, Tend3,
     Agoal1, Agoal2, Agoal3, 
     Tmin, Tmax, Tstep, 
     Amin, Amax, Astep;

function simulate (form_id, plot_id, width, height)
{
    var  forms = document.getElementById(form_id);
    var  plots = document.getElementById(plot_id);

    V0      = parseFloat(forms.V0.value);
    Omega0  = parseFloat(forms.Omega0.value);
    I0      = parseFloat(forms.I0.value);
    R       = parseFloat(forms.R.value);
    K       = parseFloat(forms.K.value);
    Jmot    = parseFloat(forms.Jmot.value);
    if (isNaN(V0) || isNaN(Omega0) || isNaN(I0) ||
        isNaN(R)  || isNaN(K)      || isNaN(Jmot) ) {
        alert("モータ特性に入力ミスがあります");
        return;
    } 

    Rho     = parseFloat(forms.Rho.value);
    Eta     = parseFloat(forms.Eta.value);
    Jgear   = parseFloat(forms.Jgear.value);
    if (isNaN(Rho) || isNaN(Eta) || isNaN(Jgear)) {
        alert("ギヤ特性に入力ミスがあります");
        return;
    } 

    Nenc    = parseFloat(forms.Nenc.value);
    if (isNaN(Nenc)) {
        alert("エンコーダ特性に入力ミスがあります");
        return;
    }

    Larm    = parseFloat(forms.Larm.value);
    Marm    = parseFloat(forms.Marm.value);
    Mweight = parseFloat(forms.Mweight.value);
    if (isNaN(Larm) || isNaN(Marm) || isNaN(Mweight)) {
        alert("負荷特性に入力ミスがあります");
        return;
    } 

    Kp      = parseFloat(forms.Kp.value);
    Ki      = parseFloat(forms.Ki.value);
    Kd      = parseFloat(forms.Kd.value);
    if (isNaN(Kp) || isNaN(Ki) || isNaN(Kd)) {
        alert("PIDパラメータに入力ミスがあります");
        return;
    } 

    DeltaT  = parseFloat(forms.DeltaT.value);
    if (isNaN(DeltaT)) {
        alert("PID制御周期に入力ミスがあります");
        return;
    } 

    Vmp     = parseFloat(forms.Vmp.value);
    if (isNaN(Vmp)) {
        alert("モータ駆動電源に入力ミスがあります");
        return;
    } 

    Tend1   = parseFloat(forms.Tend1.value);
    Agoal1  = parseFloat(forms.Agoal1.value);
    Tend2   = parseFloat(forms.Tend2.value);
    Agoal2  = parseFloat(forms.Agoal2.value);
    Tend3   = parseFloat(forms.Tend3.value);
    Agoal3  = parseFloat(forms.Agoal3.value);
    if (isNaN(Tend1)  || isNaN(Tend2)  || isNaN(Tend3) ||
        isNaN(Agoal1) || isNaN(Agoal2) || isNaN(Agoal3) ) {
        alert("制御指令に入力ミスがあります");
        return;
    } 

    Tmin  = parseFloat(forms.Tmin.value);
    Tmax  = parseFloat(forms.Tmax.value);
    Tstep = parseFloat(forms.Tstep.value);
    Amin  = parseFloat(forms.Amin.value);
    Amax  = parseFloat(forms.Amax.value);
    Astep = parseFloat(forms.Astep.value);
    if (isNaN(Tmin) || isNaN(Tmax) || isNaN(Tstep) ||
        isNaN(Amin) || isNaN(Amax) || isNaN(Astep) ) {
        alert("グラフ設定に入力ミスがあります");
        return;
    } 

    gopen(plot_id, 
          Tmin, Tmax, Tstep,  Amin, Amax, Astep, 
          width, height,  25, 25, 40, 50);

    cont_init();
    cont_loop(Agoal1, Tend1);
    cont_loop(Agoal2, Tend2);
    cont_loop(Agoal3, Tend3);

    gclose();
}

function sim_init (form_id, plot_id, width, height)
{
    var  forms = document.getElementById(form_id);

    set_motor(form_id, "118637");

    Tmin  = parseFloat(forms.Tmin.value);
    Tmax  = parseFloat(forms.Tmax.value);
    Tstep = parseFloat(forms.Tstep.value);
    Amin  = parseFloat(forms.Amin.value);
    Amax  = parseFloat(forms.Amax.value);
    Astep = parseFloat(forms.Astep.value);

    gopen(plot_id, 
          Tmin, Tmax, Tstep,  Amin, Amax, Astep, 
          width, height,  25, 25, 40, 50);
    gclose();
}