// This may look like C code, but it is really -*- C++ -*-
// RTSS --- Railway Total System Simulator
// (c) TAKAGI Ryo (at Kogakuin University)
// notchr.cc --- class train functions: notchrate()
// -----
// ChangeLog:
// 2007. 11. 21
//  Redirected log messages to l_file.
// -----

#include <cstdio>
#include <cstdlib>
#include <cmath>

#include "train.hh"
#include "StrPrintf.hh"

#define INVLIM 1e-5

//#define DEBUG
//#define CVPRINT	// V1, V2 の値を吐き出す

#define NO_JERK

#define MINCL	10.0
#define REGEN_OFF_TETA	5.0



// フルノッチ比の決定
void
train :: notchrate
()
{
  rnotchold = rnotch ;		// 古い値の保存

  switch ( stat )
  {
  case Power :			// 力行中
    nr_power () ;
    break ;

  case Brake :
  case Regenerate :		// 回生中
    nr_regen () ;
    break ;

  case Coast :			// 惰行中
    nr_coast () ;
#ifdef DEBUG
    l_ofs << StrPrintf
      ( "car No.%i:coast,nr=%6.3f, v=%7.1f" , carno , rnotch , v ) << endl ;
#endif	
    break ;

  case Const_vel :		// 定速走行中
    nr_constv () ;
    break ;

  case Stn_stop :
  case Between_stop :
  default :
    rnotch = dnch = 0.0 ;	// どちらもゼロ
  }
#ifdef NO_JERK
#else
  b_jerk = false ;
  if ( fabs ( rnotch - rnotchold ) > 0.25 && stat == stat_old )
  {
    // 状態遷移以外の場合、フルノッチ比率の急変を避ける
    rnotch = ( rnotch - rnotchold ) / fabs ( rnotch - rnotchold ) ;
    rnotch *= 0.25 ;
    rnotch += rnotchold ;
    dnch = 0.0 ;
    b_jerk = true ;
  }
#endif
}


// notchrate のサブルーチン、力行時
void
train :: nr_power
( double & rnch ,
  double & dc )
{
  tetavisub ( 1.0 ) ;

  switch ( ccmd )
  {
  case Ec_auto :
  case Ea_cvauto0 :
    nr_p_ea ( rnch , dc ) ;
    break ;

  case None :
  default :	// default: -Wall warning 回避のため
    rnch = 1.0 ;
    dc = 0.0 ;
    break ;
  }
}



// notchrate() の subroutine, 定速走行時
void
train :: nr_constv
()
{
  tetavisub ( 0.0 ) ;

  switch ( ccmd )
  {
  case None :
  default :		// default: -Wall warning 回避のため
  case Ec_auto :	// こちらの場合は再検討の必要あり
  case Ea_cvauto0 :
    double tres = tresist ( vel , pgrad ) ;
    train_ParamFunc * __pf = getNewParamFuncCalculator () ;
    __pf -> prepareMaximumEffort () ;
    double tfp = __pf -> getMaxTractiveEffort () ;
    double tfb = __pf -> getMaxBrakingEffort () ;
    delete __pf ;
//     double tfp = tracf ( vel , v , 1.0 ) ;
//     sw_dtracf = true ;
//     double tfb = tracf ( vel , v , - 1.0 ) ;
//     sw_dtracf = false ;
    if ( ( fabs ( tfp ) <= fabs ( tres ) || fabs ( tfp ) < INVLIM )
         && ( tres > 0 ) )
    {
      rnotch = 1.0 ;
      dnch = 0.0 ;
    }
    else if ( ( fabs ( tfb ) <= fabs ( tres ) || fabs ( tfb ) < INVLIM )
              && ( tres < 0 ) )
    {
      rnotch = - 1.0 ;
      dnch = 0.0 ;
    }
    else if ( tres >= 0.0 )
    {
      rnotch = fabs ( tres / tfp ) ;
      double vlow = vlow_p ;
      double vhigh = vhigh_p ;
      double vl = vlow * v / pdata . ec ;
      double vh = vhigh * v / pdata . ec ;
      if ( vel < vl )
      {
        dnch = 0.0 ;
      }
      else if ( vel < vh )
      {
        dnch = - rnotch / v ;
      }
      else
      {
        dnch = - 2.0 * rnotch / v ;
      }
    }
    else
    {
      rnotch = - fabs ( tres / tfb ) ;
      // dnch = 0.0 ;
      double vlow = vlow_b ;
      double vhigh = vhigh_b ;
      double vl = vlow * v / bdata . ec ;
      double vh = vhigh * v / bdata . ec ;
      if ( vel < bdata . regenoff )
      {
        dnch = 0.0 ;
      }
      else if ( vel < vl )
      {
        dnch = 0.0 ;
      }
      else if ( vel < vh )
      {
        dnch = - rnotch / v ;
      }
      else
      {
        dnch = - 2.0 * rnotch / v ;
      }
    }
  }
}



// notchrate の subroutine, 回生時
void
train :: nr_regen
()
{
  tetavisub ( - 1.0 ) ;

  switch ( ccmd )
  {
  case None :
  case Ec_auto :
  case Ea_cvauto0 :
  default :		// default: -Wall warning 回避のため
    train_ParamFunc * __pf = getNewParamFuncCalculator () ;
    __pf -> prepareMaximumEffort () ;
    double tfx = __pf -> getMaxBrakingEffort () ;
    delete __pf ;
//     sw_dtracf = true ;
//     double tfx = tracf ( vel , v ) ;	// 負値！絞り込み考慮せず
//     sw_dtracf = false ;
    if ( fabs ( tfx ) < INVLIM )
    {
      rnotch = - 1.0 ;	// 電気的な力はどうせ出ないが、いちおう -1
      dnch = 0.0 ;
      break ;
    }
    double bkf = brakef () ;		// 正値！
    if ( bkf > fabs ( tfx ) )
    {
      rnotch = - 1.0 ;
      dnch = 0.0 ;
      break ;
    }
    double rnx = bkf / tfx ;	// 負値！
    if ( rnx > 0.0 )
    {
      rnotch = 0.0 ;
      dnch = 0.0 ;
    }
    else
    {
      rnotch = rnx ;
      double vlow = vlow_b ;
      double vhigh = vhigh_b ;
      double vl = vlow * v / bdata . ec ;
      double vh = vhigh * v / bdata . ec ;
      if ( vel < bdata . regenoff )
      {
        dnch = 0.0 ;
      }
      else if ( vel < vl )
      {
        dnch = 0.0 ;
      }
      else if ( vel < vh )
      {
        dnch = - rnotch / v ;
      }
      else
      {
        dnch = - 2.0 * rnotch / v ;
      }
    }
    break ;
  }
}



// notchrate の subroutine, 惰行時
void
train :: nr_coast
()
{
  tetavisub ( 0.0 ) ;
  cpsww = false ;

  switch ( ccmd )
  {
  case None :
    rnotch = dnch = 0.0 ;
    break ;

  case Ec_auto :
  case Ea_cvauto0 :
    nr_c_ea () ;
    break ;

  default :
    // tetavisub ( 0.0 ) ;
    rnotch = dnch = 0.0 ;
    break ;
  }
}



// notchrate() の subprogram, Ecauto 力行時
void
train :: nr_p_ea
( double & rnch ,
  double & dc )
{
  double rnx ; 	// 「最小フルノッチ比」

  // 速度が低過ぎれば rnch = 1.0
  // 電圧が高ければ rnch = 1.0
  if ( vel < adata . minvel || v > adata . pminvol )
  {
    rnch = 1.0 ;
    dc = 0.0 ;
    return ;
  }
  if ( vel < adata . fullvel )
  {
    // 最小 rnotch
    rnx = vel - adata . minvel ;
    rnx /= adata . fullvel - adata . minvel ;
  }
  else
  {
    rnx = 1.0 ;
  }
  
  rnx = 1.0 - rnx ;
  if ( v < adata . poffvol )
  {
    // 電圧低過ぎ: rnch = rmin
    rnch = rnx ;
    dc = 0.0 ;
    return ;
  }

  rnch = v - adata . poffvol ;	// 0 < rnch 1
  rnch /= adata . pminvol - adata . poffvol ;
  rnch *= 1.0 - rnx ;
  rnch += rnx ;			// rnch を定める
  dc = ( 1.0 - rnx ) / ( adata . pminvol - adata . poffvol ) ;
}



// フルノッチ比の決定、ccmd = Ecauto、惰行時
void
train :: nr_c_ea
()
{
  double rnx , rnbx ;
  if ( vel < adata . minvel
       || ( v >= adata . pminvol && v <= adata . crbvol ) )
  {
    rnotch = dnch = 0.0 ;	// 速度が低過ぎれば rnotch = 0.0
    return ;             	// 電圧が中くらいなら rnotch = 0.0
  }

  // ここまで来たらかならず惰行車の状態遷移
  if ( vel < adata . fullvel )
  {
    rnx = vel - adata . minvel ;
    rnx /= adata . fullvel - adata . minvel ;
  }
  else
  {
    rnx = 1.0 ;
  }
  // 最小フルノッチ比を定める

  // ブレーキ時最小フルノッチ比
  train_ParamFunc *__pf = getNewParamFuncCalculator () ;
  __pf -> prepareMaximumEffort () ;
  double tfb = __pf -> getMaxBrakingEffort () ;
  delete __pf ;
  rnbx = fabs ( brakef () / tfb ) ;
//   rnbx = fabs ( brakef () / tracf ( vel , v ) ) ;

  // rnotch を定める
  if ( v < adata . poffvol )
  {
    // 電圧が非常に低ければフル回生
    rnotch = - fabs ( rnx ) ;
    if ( fabs ( rnx ) > rnbx )
    {
      rnotch = - rnbx ;
    }

    dnch = 0.0 ;
  }
  else if ( v < adata . pminvol )
  {
    // 電圧が低ければ少し回生
    rnotch = v - adata . pminvol ;
    rnotch /= adata . poffvol - adata . pminvol ;
    rnotch *= - rnx ;
    dnch = rnx / ( adata . pminvol - adata . poffvol ) ;
    if ( fabs ( rnotch ) > rnbx )
    {
      rnotch = - rnbx ;
      dnch = 0.0 ;
    }
  }
  else if ( v > adata . crfvol )
  {
    // 電圧が非常に高ければフル力行
    rnotch = rnx ;
    dnch = 0.0 ;
  }
  else
  {
    // 電圧が高ければ少し力行
    rnotch = v - adata . crbvol ;
    rnotch /= adata . crfvol - adata . crbvol ;
    rnotch *= rnx ;
    dnch = rnx / ( adata . crfvol - adata . crbvol ) ;
  }
  if ( fabs ( rnotch ) > INVLIM )
  {
    cpsww = true ;
  }
}
