#include  "trs5-inc.mm"

Real limiter();

Func Matrix ftrs5(t, x)
     // ftrs5 - 微分方程式の微分値を表す式
     Real t;
     Matrix x;
{
// 線路事故の段階
  static Integer  f_fault_step,   f_last_fault_step;
  Real  f_reduction_rate;    	// 事故継続中の線路定数の計算に使う

  Matrix dx;

  // 機械入力
  Pm1= Pms1;

  // 事故のフェイズによってパラメータ値を変化させる.
  if (t<= fault_time){
    // 元々の状態
    a11 = 1.0;
    a21 = 0.0;
    a31 = Re1;
    a41 = Xtrans1 + Xline1;
    f_fault_step= 1;
  }else if (t<= fault_time + fault_duration){
    // 三相短絡が生じた状態
    a11 = fault_position/(1.0+fault_position);		// b/(1+b)
    a21 = 0.0;
    // f_reduction_rate = kr = (1-a+b+ab)/(1+b)
    f_reduction_rate = (1.0 - twoline_rate + fault_position + twoline_rate*fault_position)/(1.0 + fault_position);
    a31 = f_reduction_rate*Re1;
    a41 = Xtrans1 + f_reduction_rate*Xline1;
    f_fault_step= 2;
  }else if (t<= fault_time + fault_duration + reclose_interval){
    // 事故を除去した状態
    a11 = 1.0;
    a21 = 0.0;
    a31 = (1.0+twoline_rate)*Re1;				// (1+a)Re
    a41 = Xtrans1 + (1.0+twoline_rate)*Xline1;		// Xtrans + (1+a)Xline
    f_fault_step= 3;
  }else{
    // 線路を再閉路した場合 i.e. 元の状態に戻る
    a11 = 1.0;
    a21 = 0.0;
    a31 = Re1;
    a41 = Xtrans1 + Xline1;
    f_fault_step= 4;
  }

  // 事故などで線路が変化した時にc0, c1, ... c6 を計算しなおす
  if  (f_fault_step != f_last_fault_step || f_fault_step == 0){
    // Et = (a1 + j a2) Vinf + (a3 + j a4) I
    // id, iq の計算に使う
    c01= a31*a31 + (Xq1 + a41)*(XdD1 + a41);
    c11= (a11*a31 + a21*(Xq1 + a41))/c01;
    c21= (a11*(Xq1 + a41) - a21*a31)/c01;
    c31= (Xq1 + a41)/c01;
    c41= (a11*(XdD1 + a41) - a21*a31)/c01;
    c51= (a11*a31 + a21*(XdD1 + a41))/c01;
    c61= a31/c01;
  }
  f_last_fault_step= f_fault_step;

  edinf1 = Vinf1*sin(x(Ndlt1));
  eqinf1 = Vinf1*cos(x(Ndlt1));
  id1= -c11*edinf1 - c21*eqinf1 + c31*x(NeqD1);
  iq1=  c41*edinf1 - c51*eqinf1 + c61*x(NeqD1);

  Pe1 = (x(NeqD1) + (Xq1 - XdD1)*id1)*iq1;
  Q1 = -(x(NeqD1)*id1 - XdD1*id1*id1 - Xq1*iq1*iq1);
  ed1= Xq1*iq1;
  eq1= x(NeqD1) - XdD1*id1;
  Vt1= sqrt(ed1*ed1 + eq1*eq1);

  ef1= limiter(x(NgV1), UlV1, LlV1);

  // 初期化せずに dx(n)=.. で代入すると, 行ベクトルになってしまう?
  dx= Z(x);

  // %%%%%%%%%%% system 1 %%%%%%%%%%%%
  dx(Ndlt1)=  wB * x(Nw1);
  dx(Nw1)=  (Pm1 - Pe1 - D1*x(Nw1))/M1;
  dx(NeqD1)=  (-x(NeqD1) -(Xd1 - XdD1)*id1 + ef1)/TdoD1;
  switch(systype){
  case FirstOrderAVR:
       dx(NgV1)= ( -(x(NgV1)-efs1) + GV1*(Vts1-Vt1) )/TV1;
       break;
  case WithVMdelay: // == default
  default:
       dx(NVvm1)=  (-x(NVvm1)+Vt1)/Tvm1;
       dx(NgV1)= ( -(x(NgV1)-efs1) + GV1*(Vts1-x(NVvm1)) )/TV1;
       break;
  }

  return dx;
}



Func Real limiter(xin, U, L)
     Real xin, U, L;
{
     if       (xin>U){
	  return U;
     }else if (xin<L){
	  return L;
     }else{
	  return xin;
     }
}