// This may look like C code, but it is really -*- C++ -*-
// RTSS --- Railway Total System Simulator
// (c) TAKAGI Ryo (at Kogakuin University)
// motion.cc --- Functions to calculate motion of trains
// -----
// ChangeLog:
// 2010. 1. 17
//  In train::tracf(...), call to ttvi_brake() is modified so that it creates
//  an instance of train::_ParamFunc.
// 2009. 11. 26
//  Fixed bug in train::brakef().
// 2007. 11. 21
//  Redirected log messages to l_file.
// -----

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

#include "train.hh"

#define MINCURVE 10

//#define DEBUG
//#define DEBUG_PASS_STATION
using std :: cerr ;
using std :: endl ;



// 列車のブレーキオフ位置、引数: pos, pgrad, vel, arv
double
train :: bklen
( double bkpos ,
  nextsta :: gc_size_type jgrad ,
  double bkvel ,
  double arv )
{
  // 列車速度が目標速度より低い →現在の位置 bkpos を返す
  if ( bkvel <= arv )
    return bkpos ;
  const double gconst = 9.80665 * 36E-4 ;
  double bvel = bkvel ;
  double bpos = bkpos ;
  nextsta :: gc_size_type i = jgrad ;
  double za = 0.0 ;
  while ( 1 )
  {
    double ab = abh + egrad ( i ) * gconst / ( 1 + eqrot () ) ;
    double c , x ;
    if ( bkpos * direc () >= nxx -> epoint ( nxx->dqgrad () ) * direc () )
    {
      c = ( bvel - arv ) / ab ;
      x = ( bvel * c - ab * c * c / 2 ) / 3600 ;
      za += x ;
      return bkpos + za * direc () ;
    }	    
    x = ( nxx -> epoint ( i ) - bpos ) * direc () ;
    double b = bvel * bvel - 7200 * ab * x ;
    if ( b < 0 )
    {
      c = ( bvel - arv ) / ab ;
      x = ( bvel * c - ab * c * c / 2 ) / 3600 ;
      za += x ;
      return bkpos + za * direc () ;
    }
    c = ( bvel - sqrt ( b ) ) / ab ;
    double va = bvel - ab * c ;
    if ( va - arv <= 0 )
    {
      if ( va - arv < 0 )
      {
	c = ( bvel - arv ) / ab ;
	x = ( bvel * c - ab * c * c / 2 ) / 3600 ;
      }
      za += x ;
      return bkpos + za * direc () ;
    }
    else
    {
      bpos = nxx -> epoint ( i ) ;
      bvel = va ;
      za += x ;
      i += direc () ;
      if ( i >= nxx -> gc_size () || i < 0 )
      {
	double retval = nxx -> epoint ( i - direc () ) + direc () ;
	return retval ;
      }
    }
  }
}



// 列車走行シミュレーション、引数: pos, pgrad, vel, tracf
void
train :: npos
( double & posnp ,
  nextsta :: gc_size_type & jgrad ,
  double & velnp ,
  double tracff /* = 0 */ )
{
#ifdef DEBUG
  cerr << "npos start" << endl ;
#endif

  // ----
  // Calculating mechanical power.
  // 機械的パワーの計算。
  // ----
  if ( _sw_mech_power )
  {
    _mech_power = velnp * tracff * 9.80665 / 3.6 ;
    _mech_power_motor_ratio = 1.0 ;
  }

  double x = 9.80665 / ( ( 1 + eqrot () ) * wall () ) ;
  x *= deltah * 3.6 ;
#ifdef DEBUG
  cerr << "x calculated" << endl ;
#endif

  double k11 = deltah * direc () * velnp / 3600 ;
  double k12 = x * ( tracff - gresist ( posnp ) - tresist ( velnp ) ) ;
#ifdef DEBUG
  cerr << "k11, k12 calculated" << endl ;
#endif
  double k21 = deltah * direc () * ( velnp + k12 / 2 ) / 3600 ;
  double k22 = x ;
  k22 *= tracff - gresist ( posnp + k11 / 2 ) - tresist ( velnp + k12 / 2 ) ;
#ifdef DEBUG
  cerr << "k21, k22 calculated" << endl ;
#endif
  double k31 = deltah * direc () * ( velnp + k22 / 2 ) / 3600 ;
  double k32 = x ;
  k32 *= tracff - gresist ( posnp + k21 / 2 ) - tresist ( velnp + k22 / 2 ) ;
#ifdef DEBUG
  cerr << "k31, k32 calculated" << endl ;
#endif
  double k41 = deltah * direc () * ( velnp + k32 ) / 3600 ;
  double k42
    = x * ( tracff - gresist ( posnp + k31 ) - tresist ( velnp + k32 ) ) ;
#ifdef DEBUG
  cerr << "k41, k42 calculated" << endl ;
#endif

  posnp += ( k11 + 2 * ( k21 + k31 ) + k41 ) / 6 ;
  velnp += ( k12 + 2 * ( k22 + k32 ) + k42 ) / 6 ;

  if ( posnp * direc () < nxx -> parrive () * direc () )
    nxx -> adjpgrad ( jgrad , posnp ) ;
  else
    posnp = nxx -> parrive () ;
#ifdef DEBUG
  cerr << "npos ended" << endl ;
#endif
}



// 列車走行シミュレーション(ブレーキ時)、引数: pos , pgrad , vel
void
train :: nposb
( double & posnp ,
  nextsta :: gc_size_type & jgrad ,
  double & velnp ,
  double tracff )
{
  // ----
  // Calculating mechanical power.
  // 機械的パワーの計算。
  // ----
  if ( _sw_mech_power )
  {
    _mech_power = - velnp * brakef ( velnp ) * 9.80665 / 3.6 ;
    _mech_power_motor_ratio = - tracff / brakef ( velnp ) ;
    if ( _mech_power_motor_ratio > 1.0 + INVLIM
         || _mech_power_motor_ratio < - INVLIM )
    {
      cerr << "Warning: Electric braking effort at wheel rim exceeds"
           << " whole braking effort of train " << car_number () << endl
           << " t_sim = " << t_sim << ", Mech Power = " << _mech_power
           << ", Ratio = " << _mech_power_motor_ratio << endl ;
      l_ofs << "Warning: Electric braking effort at wheel rim exceeds"
            << " whole braking effort of train " << car_number () << endl
            << " t_sim = " << t_sim << ", Mech Power = " << _mech_power
            << ", Ratio = " << _mech_power_motor_ratio << endl ;
    }
    if ( _mech_power_motor_ratio > 1.0 )
    {
      _mech_power_motor_ratio = 1.0 ;
    }
    if ( _mech_power_motor_ratio < 0.0 )
    {
      _mech_power_motor_ratio = 0.0 ;
    }
  }

  double ba = abh + egrad ( jgrad ) * 9.80665 * 36E-4 / ( 1 + eqrot () ) ;
  posnp += direc () * ( velnp * deltah * 2 - ba * deltah * deltah ) / 7200 ;
  velnp -= deltah * ba ;
  if ( posnp * direc () < nxx -> parrive () * direc () )
    nxx -> adjpgrad ( jgrad , posnp ) ;
  else
    posnp = nxx -> parrive () ;
}



// 等価勾配の計算（位置から）
double
train :: egrad
( double pos )
{
  nextsta :: gc_size_type jgrad = nxx -> dpgrad ( pos ) ;
  return egrad ( jgrad ) ;
}



// 等価勾配の計算（jgrad から）
double
train :: egrad
( nextsta :: gc_size_type jgrad )
{
  double egradi = nxx -> grad ( jgrad ) * direc () ;
  if ( nxx -> curve ( jgrad ) > MINCURVE )
  {
    egradi += cresist / nxx -> curve ( jgrad ) ;
  }
  return egradi ;
}



// 所要ブレーキ力
double
train :: brakef
()
{
  double resist = tresist ( vel ) /* + gresist () */ ;
  return wall () * ( 1.0 + eqrot () ) * abh / ( 3.6 * 9.80665 ) - resist ;
}



// 所要ブレーキ力
double
train :: brakef
( double velnp )
{
  double resist = tresist ( velnp ) /* + gresist () */ ;
  return wall () * ( 1.0 + eqrot () ) * abh / ( 3.6 * 9.80665 ) - resist ;
}



// 列車走行シミュレーション(定速走行時)、引数: pos , pgrad , vel
void
train :: nposc
( double & posnp ,
  nextsta :: gc_size_type & jgrad ,
  double & velnp ,
  double tracff )
{
  // ----
  // AMANO_B: Insert here a routine to cope with train current capping. Mode
  // change due to current capping must be done in train::nm_constv() member
  // function.
  // ----
  train_ParamFunc *__pf = getNewParamFuncCalculator () ;
  __pf -> prepareMaximumEffort () ;
  double tfa = __pf -> getMaxTractiveEffort () ;
  double tfb = tresist ( velnp , jgrad ) ;
  delete __pf ;
  if ( tfa > tfb )
  {
    posnp += direc () * deltah * velnp / 3600.0 ;
    // ----
    // Calculating mechanical power.
    // 機械的パワーの計算。
    // ----
    if ( _sw_mech_power )
    {
      _mech_power = velnp * tfb * 9.80665 / 3.6 ;
      _mech_power_motor_ratio = tracff / tfb ;
      if ( _mech_power_motor_ratio > 1.0 + INVLIM
           || _mech_power_motor_ratio < - INVLIM )
      {
        cerr << "Warning: Electric braking effort at wheel rim exceeds"
             << " whole braking effort of train " << car_number () << endl
             << " t_sim = " << t_sim << ", Mech Power = " << _mech_power
             << ", Ratio = " << _mech_power_motor_ratio << endl ;
        l_ofs << "Warning: Electric braking effort at wheel rim exceeds"
              << " whole braking effort of train " << car_number () << endl
              << " t_sim = " << t_sim << ", Mech Power = " << _mech_power
              << ", Ratio = " << _mech_power_motor_ratio << endl ;
      }
      if ( _mech_power_motor_ratio > 1.0 )
      {
        _mech_power_motor_ratio = 1.0 ;
      }
      if ( _mech_power_motor_ratio < 0.0 )
      {
        _mech_power_motor_ratio = 0.0 ;
      }
    }
  }
  else
  {
    npos ( posnp , jgrad , velnp , tfa ) ;
  }
#ifdef DEBUG_PASS_STATION
  cerr << "nposc: car " << car_number () << ": posnp = " << posnp << endl ;
  l_ofs << "nposc: car " << car_number () << ": posnp = " << posnp << endl ;
  cerr << "nposc: car " << car_number () << ": parrive = "
       << nxx -> parrive () << endl ;
  l_ofs << "nposc: car " << car_number () << ": parrive = "
        << nxx -> parrive () << endl ;
#endif
  if ( posnp * direc () >= nxx -> parrive () * direc () )
  {
    posnp = nxx -> parrive () ;
  }
  nxx -> adjpgrad ( jgrad , posnp ) ;
}



// ----
// Preparing maximum values for the tractive and braking effort at the
// velocity and voltage. Regeneration limiter will not be considered.
// ある速度および電圧における列車の牽引力の計算。回生絞込は考慮されない。
// ----
void
train_ParamFunc :: prepareMaximumEffort
()
{
  // ----
  // Acceleration.
  // 加速.
  // ----
  double vlow = __vlow_p ;
  double vhigh = __vhigh_p ;
  double constf = __constf_p ;	// _p が重要、力行用を示す
  double vl = vlow * __v / __pd . ec ;	// 電圧で補正
  double vh = vhigh * __v / __pd . ec ;	// 電圧で補正
  if ( __vel < vl )
  {
    __max_tractive_effort = constf ;
  }
  else if ( __vel < vh )
  {
    __max_tractive_effort = constf * vl / __vel ;
  }
  else
  {
    __max_tractive_effort = constf * vl * vh / ( __vel * __vel ) ;
  }
  __max_tractive_effort *= __rnotch_for_capper ;

  // ----
  // Braking.
  // 減速.
  // ----
  vlow = __vlow_b ;
  vhigh = __vhigh_b ;
  constf = __constf_b ;	// _b が重要、ブレーキ用を示す
  vh = vhigh * __v / __bd . ec ;
  vl = vlow * __v / __bd . ec ;
  if ( __vel < __bd . regenoff )
  {
    __max_braking_effort = 0.0 ;
  }
  else if ( __vel < vl )
  {
    __max_braking_effort = constf ;
  }
  else if ( __vel < vh )
  {
    __max_braking_effort = constf * vl / __vel ;
  }
  else
  {
    __max_braking_effort = constf * vl * vh / ( __vel * __vel ) ;
  }
  __max_braking_effort *= __rnotch_for_capper ;
}



// ----
// Getting the tractive effort of a train at a certain set of velocity,
// voltage and "notch rate". Will not observe sw_dtracf. Execute member
// function prepareMaximumEffort() before executing this function.
// ある速度・電圧・「フルノッチ比」の組合せにおける列車の牽引力の計算。
// sw_dtracf 変数は見ない（常に回生絞込計算を行う）。この館数の実行前に，まず
// prepareMaximumEffort() メンバ関数を実行すること。
// ----
double
train_ParamFunc :: getTractiveEffort
()
  const
{
  if ( __rnotch > 2.0 || __rnotch < - 2.0 )
  {
    cerr << "Error: rnotch out of range in" << endl
         << "train::train_ParamFunc::getTractiveEffort()" << endl ;
    exit ( 24 ) ;		// error 24: rnotch 設定エラー
  }
  if ( __rnotch <= INVLIM && __rnotch >= - INVLIM ) 
  {
    return 0.0 ;
  }
  else if ( __rnotch > INVLIM )
  {
    // Accelerating.
    return __rnotch * __max_tractive_effort ;
  }
  else
  {
    // Regenerating.
    double retval = __rnotch * __max_braking_effort ;
    // ----
    // Begin: Added 16 Jan 2010
    // ----
    train_ParamFunc * __pf = new train_ParamFunc ( * this ) ;
    __pf -> setVelocity ( __vel ) ;
    __pf -> setVoltage ( __v ) ;
    __pf -> setNotchRate ( __rnotch ) ;
    __pf -> ttvi_brake () ;
    double amp = __pf -> getMainCircuitCurrent () ;
    double damp = __pf -> get_Di_Dtheta () ;
    double dnoshibo = __pf -> getCurrentWithoutLimiter () ;
    delete __pf ;
    // ----
    // End: Added 16 Jan 2010
    // ----
    if ( dnoshibo > amp )
    {
      retval *= amp / dnoshibo ;
    }
    if ( __v > __bd . regendvol )
    {
      retval = 0.0 ;
    }
    return retval ;		// 値を返す
  }
}



// ----
// Getting the tractive effort of a train.
// 列車の牽引力の計算。
// ----
double
train :: getTractiveEffort
()
  const
{
  train_ParamFunc * __pf = getNewParamFuncCalculator () ;
  __pf -> prepareMaximumEffort () ;
  double retval = __pf -> getTractiveEffort () ;
  delete __pf ;
  return retval ;
}
