// This may look like C code, but it is really -*- C++ -*-
// RTSS --- Railway Total System Simulator
// (c) TAKAGI Ryo (at Prof. SONE's Laboratory, the Univ. of Tokyo.)
// ontimsub.cc --- on-time notchoff position calculator: subroutines
// $Id: ontimsub.cc,v 2.4 1994/12/27 12:03:47 rt Exp $

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

#include "train.hh"

//#define DEBUG

#define SOKUDOIHAN 0.00

#ifdef DEBUG
#include "StrPrintf.hh"
#endif

const double zconst = 0.0;
const double gconst = 9.80665*3.6;



// ontime のサブプログラム，Const_vel -> Brake
double
train :: ontbc
( double p ,
  nextsta :: gc_size_type j ,
  double v ,
  double t )
{
  nxx -> adjpgrad ( j , p ) ;
  double ab = abh + egrad ( j ) * gconst / ( ( 1 + eqrot() ) * 1e3 ) ;
  nextsta :: gc_size_type i = 0 ;
  double ap ;
  if ( j == nxx -> dqgrad () )
  {
    ap = nxx -> parrive () ;
    i = nxx -> dqgrad () ;
  }
  else
  {
    ap = nxx -> epoint ( j) ;
    i = j ;
  }
  double av , bt ;
  brake ( nxx -> varrive () , nxx -> parrive () , nxx -> dqgrad () ,
          ap , i , av , bt ) ;
  bool sw ;
  double t1 , v1 ;
#ifdef DEBUG
  l_ofs << "car " << carno << ", before pbx in ontbc" << endl ;
#endif
  pbx ( av , v , ap , p , ab , zconst , t1 , v1 , sw ) ;
  if ( sw == true ) return t + nxx -> tarrive ( pfirst ) * 10.0 ;
  return t + t1 + bt ;
}



// ontime のサブプログラム，Coast or Const_vel
void
train :: ontcm
( double & p ,
  nextsta :: gc_size_type & j ,
  double & v ,
  double & t ,
  double & px ,
  double & vx ,
  double & tx )
{
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "ontcm, %.3f/%i/%.3f/%.2f/%.1f/%.5f" ,
                       p , j , v , t , nxx -> vlimit ( j ) ,
                       tresist ( v ) + gresist ( j ) ) << endl ;
#endif
  double ps = p ;
  double vs = v ;
  double ts = t ;
  nextsta :: gc_size_type jg = j ;
  if ( j == nxx -> dqgrad () )
  {
    // 終点を定める
    p = nxx -> parrive () ;
  }
  else
  {
    p = nxx -> epoint ( j ) ;
    j += direc () ;
  }
  double lg = direc () * ( p - ps ) ;			// 区間長さ

  // 定速でよい場合簡単な計算をして return
  if ( v >= nxx -> vlimit ( jg ) - INVLIM && nxx -> vlimit ( jg ) > INVLIM
       && tresist ( v ) + gresist ( jg ) < INVLIM )
  {
    v = nxx -> vlimit ( jg ) ;
    t += lg / v * 3600.0 ;
    px = ps ;
    vx = vs ;
    tx = ts ;
    return ;
  }
  bool sw ;
  double t1 , v1 ;
  double r = tresist ( v ) + gresist ( jg ) ;    // 第１回計算
  double aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
  cavt ( v , aa , lg , t1 , v1 , sw ) ;
  if ( sw == true )
  {
    // 計算失敗時は大きめの値を与えてリターン
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    px = p ;
    vx = v ;
    tx = t ;
    return ;
  }
  double t2 , v2 ;
  r = tresist ( v1 ) + gresist ( jg ) ;	    // 第２回計算
  aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
  cavt ( v , aa , lg , t2 , v2 , sw ) ;
  if ( sw == true )
  {
    // 計算失敗時は大きめの値を与えてリターン
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    px = p ;
    vx = v ;
    tx = t ;
    return ;
  }
  v = ( v1 + v2 ) / 2.0 ;
  t = ( t1 + t2 ) / 2.0 ;
  t += ts ;
  if ( v > nxx -> vlimit ( jg ) - INVLIM
       && nxx -> vlimit ( jg ) > INVLIM )
  {
    // 速度超過時計算やり直し
    if ( ! ( v - nxx -> vlimit ( jg ) < SOKUDOIHAN ) )
    {
      // あまり速度が変わらない時は行なわない
      v = vs ;
      t = ts ;
      ontctoc ( p , ps , jg , v , t , px , vx , tx ) ;
    }
  }
  else
  {
    px = p ;
    vx = v ;
    tx = t ;
  }
}




// ontime のサブプログラム，Coast -> Const_vel
void
train :: ontctoc
( double ps ,
  double & p ,
  nextsta :: gc_size_type j ,
  double & v ,
  double & t ,
  double & px ,
  double & vx ,
  double & tx )
{
  double t1 , px1 , vx1 , tx1 ;
  bool sw ;
  double r = tresist ( v ) + gresist ( j ) ; // 第１回計算
  double aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
  if ( fabs ( nxx -> vlimit ( j ) - v ) <= INVLIM )
  {
    v = nxx -> vlimit ( j ) ;
    px = p ;
    vx = v ;
    tx = t ;
    t += fabs ( ps - p ) / v * 3600.0 ;
    return ;
  }
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "before pbx in ontctoc ... first, aa = %.6f", aa )
        << endl ;
#endif
  pbx ( nxx -> vlimit ( j ) , v , ps , p , zconst , aa , t1 , vx1 , sw ,
        px1 , tx1 ) ;
  if ( sw == true )
  {
    // 計算失敗時は大きめの値を与えてリターン
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    px = p ;
    vx = v ;
    tx = t ;
    return ;
  }
  double t2 , px2 , vx2 , tx2 ;
  r = tresist ( vx1 ) + gresist ( j ) ;	    // 第２回計算
  aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << "before pbx in ontctoc ... second" << endl ;
#endif
  pbx ( nxx -> vlimit ( j ) , v , ps , p , zconst , aa , t2 , vx2 , sw ,
        px2 , tx2 ) ;
  if ( sw == true )
  {
    // 計算失敗時は大きめの値を与えてリターン
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    px = p ;
    vx = v ;
    tx = t ;
    return ;
  }
  t += ( t1 + t2 ) / 2.0 ;
  v = nxx -> vlimit ( j ) ;
  px = ( px1 + px2 ) / 2.0 ;
  vx = ( vx1 + vx2 ) / 2.0 ;
  tx = ( tx1 + tx2 ) / 2.0 ;
}




// ontime のサブプログラム，Brake 開始から停車まで
double
train :: ontbrake
( double p ,
  nextsta :: gc_size_type j ,
  double v ,
  double t )
{
  nxx -> adjpgrad ( j , p ) ;
  double ab = abh + egrad ( j ) * gconst / ( ( 1 + eqrot () ) * 1e3 ) ;
  int i = 0 ;
  double ap ;
  if ( j == nxx -> dqgrad () )
  {
    ap = nxx -> parrive () ;
    i = nxx -> dqgrad () ;
  }
  else
  {
    ap = nxx -> epoint ( j ) ;
    i = j ;
  }
  double av , bt ;
  brake ( nxx -> varrive () , nxx -> parrive () , nxx -> dqgrad () ,
          ap , i , av , bt ) ;
  bool sw ;
  double t1 , v1 ;
  double r = tresist ( v ) + gresist ( j ) ;
  double aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << "before pbx in ontbrake ... first" << endl ;
#endif
  pbx ( av , v , ap , p , ab , aa , t1 , v1 , sw ) ;
  if ( sw == true )
  {
    return t + nxx -> tarrive ( pfirst ) * 10.0 ;
  }

  double t2 , v2 ;
  r = tresist ( v1 ) + gresist ( j ) ;
  aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << "before pbx in ontbrake ... second" << endl ;
#endif
  pbx ( av , v , ap , p , ab , aa , t2 , v2 , sw ) ;
  if ( sw == true )
  {
    return t + nxx -> tarrive ( pfirst ) * 10.0 ; 
  }

  t = ( t1 + t2 ) / 2.0 + bt + t ;
  return t ;
}




// ontime のサブプログラム，前方速度制限の発見
bool
train :: ontvlim
( double np ,
  nextsta :: gc_size_type pg ,
  double nv ,
  double & nxarp ,
  nextsta :: gc_size_type & k ,
  double & nxarv )
{
  //    bool sw = true;	// -Wall warning: unused variable 27Sep94
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "ontvlim: np , nv = %.3f %.1f", np , nv ) << endl ;
#endif
  nextsta :: gc_size_type i = 0 ;
  for ( i = nxx -> dqgrad () ;
        int ( i ) * direc () > int ( pg ) * direc () ; i -= direc () )
  {
    if ( nxx -> vlimit ( i ) <= INVLIM ||			// 制限なし
         nxx -> vlimit ( i ) >= nxx -> vlimit ( pg ) )		// 制限より高い
      continue ;
    double bl = bklen ( np , pg , nv , nxx -> vlimit ( i ) ) * direc () ;
    if ( bl > nxx -> spoint ( i ) * direc () 
         + 10.0 * INVLIM )
    {
      // 前方制限速度減速
      k = i - direc () ;
      nxarp = nxx -> spoint ( i ) ;
      nxarv = nxx -> vlimit ( i ) ;
#ifdef DEBUG
      l_ofs << "car " << carno << ", "
        << StrPrintf ( "call ontvlc, bl,spoint = %.4f %.4f" ,
                           bl , nxarp ) << endl ;
#endif
      return true ;
    }
  }
  return false ;
}




// ontime のサブプログラム
void
train :: ontvlc
( double np ,
  nextsta :: gc_size_type nj ,
  double nv ,
  double nt ,
  double & p ,
  nextsta :: gc_size_type & j ,
  double & v ,
  double & t ,
  double arp ,
  nextsta :: gc_size_type k ,
  double arv ,
  bool zerosw /* = false */ )
{
  p = np ;
  j = nj ;
  v = nv ;
  t = nt ;
  nxx -> adjpgrad ( j , p ) ;
  double ab = abh + egrad ( j ) * gconst / ( ( 1 + eqrot () ) * 1e3 ) ;
  int i ;
  double ap ;
  if ( j == k )
  {
    ap = arp ;
    i = k ;
  }
  else
  {
    ap = nxx -> epoint ( j ) ;
    i = j ;
  }
  double av , bt ;
  brake ( arv , arp , k , ap , i , av , bt ) ;

  bool sw ;
  double t1 , v1 ;
  double aa ;
  if ( zerosw )
  {
    aa = 0 ;
  }
  else
  {
    double r = tresist ( v ) + gresist ( j ) ;
    aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
  }
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << "before pbx in ontvlc ... first" << endl ;
#endif
  pbx ( av , v , ap , p , ab , aa , t1 , v1 , sw ) ;
  if ( sw == true )
  {
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << "ontvlc, sw=true in First" << endl ;
#endif
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    return ;
  }

  double t2 , v2 ;
  if ( zerosw )
  {
    aa = 0 ;
  }
  else
  {
    double r = tresist ( v1 ) + gresist ( j ) ;
    aa = - r * gconst / ( ( 1.0 + eqrot () ) * wall () ) ;
  }
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << "before pbx in ontvlc ... second" << endl ;
#endif
  pbx ( av , v , ap , p , ab , aa , t2 , v2 , sw ) ;
  if ( sw == true )
  {
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << "ontvlc, sw=true in Second" << endl ;
#endif
    t += nxx -> tarrive ( pfirst ) * 10.0 ;
    return ;
  }
  t += ( t1 + t2 ) / 2.0 + bt ;
  p = arp ;
  j = k ;
  nxx -> adjpgrad ( j , p ) ;
  v = arv ;
}




// ontime のサブプログラム
void
train :: pbx
( double av ,
  double v ,
  double ap ,
  double p ,
  double ab ,
  double a ,
  double & t ,
  double & vb ,
  bool& sw )
{
  double za , zb ;
  pbx ( av , v , ap , p , ab , a , t , vb , sw , za , zb ) ;
}




// ブレーキ開始点を含む区間の計算
void
train :: pbx
( double av ,
  double v ,
  double ap ,
  double p ,
  double ab ,
  double a ,
  double & t ,
  double & vb ,
  bool & sw ,
  double & pb ,
  double & tx )
{
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "pbx, %.1f/%.1f/%.1f/%.1f/%.1f/%.1f/%.1f" ,
                      av , v , ap , p , ab , a , t ) << endl ;
#endif
  double q , r , s , a1 , a2 ;
  s = v / 3600.0 ;
  s *= s ;
  r = av / 3600.0 ;
  r *= r;
  a1 = a / 3600.0 ;
  a2 = ab / 3600.0 ;
  sw = false ;
  if ( fabs ( a ) <= INVLIM && fabs ( ab ) > INVLIM )
  {
    vb = v ;
    q = ( vb - av ) / ab ;				// 減速時間
    pb = q * av / 3600.0 + q * q * ab / 7200.0 ;	// ブレーキ
    pb *= direc () ;					// 開始地点
    pb = ap - pb ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "Mode01: vb, pb, q = %.1f/%.1f/%.1f" , vb , pb , q )
          << endl ;
#endif
  }
  else if ( fabs ( ab ) <= INVLIM && fabs ( a ) > INVLIM )
  {
    vb = av ;
    q = ( vb - v ) / a ;			// 惰行時間
    pb = q * v / 3600.0 + q * q * a / 7200.0 ;	// 定速走行
    pb *= direc () ;				// 開始地点
    pb += p ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "Mode10: vb, pb, q = %.1f/%.1f/%.1f" , vb , pb , q )
          << endl ;
#endif
  }
  else if ( fabs ( ab ) <= INVLIM && fabs ( a ) <= INVLIM
            || fabs ( ab + a ) <= INVLIM )
  {
    // 始点を与える
    pb = p ;
    vb = v ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "Mode00: vb, pb = %.1f/%.1f" , vb , pb ) << endl ;
#endif
  }
  else
  {
    q = 2 * a1 * a2 / ( a1 + a2 ) ;
    q *= direc () * ( ap - p ) + r / ( 2 * a2 ) + s / ( 2 * a1 ) ;
    pb = p + direc () * ( q - s ) / ( 2 * a1 ) ;
    if ( q > INVLIM )
      sw = false ;
    else if ( q >= - INVLIM )
    {
      sw = false ;
      q = 0.0 ;
    }
    else
    {
      sw = true ;
#ifdef DEBUG
      l_ofs << "car " << carno << ", "
        << StrPrintf ( "pbx Fault: q = %.5f", q ) << endl ;
#endif
      return ;
    }
    vb = sqrt ( q ) * 3600.0 ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "Mode11: vb, pb = %.1f/%.1f", vb, pb ) << endl ;
#endif
  }
  if ( pb * direc () < p * direc () - INVLIM * direc ()
       || pb * direc () > ap * direc () + INVLIM * direc () )
  {
    sw = true ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "pbx Fault: pb out of range," ) ;
    l_ofs << StrPrintf ( " pb,p,ap = %.3f %.3f %.3f", pb , p , ap ) << endl ;
#endif
    return ;
  }
  else if ( pb * direc () < p * direc () ) 
  {
    pb = p ;
  }
  else if ( pb * direc () > ap * direc () ) 
  {
    pb = ap ;
  }
  if ( fabs ( a ) > INVLIM ) 
  {
    tx = ( vb - v ) / a ;
  }
  else
  {
    tx = fabs ( p - pb ) / vb * 3600.0 ;
  }
  if ( fabs ( ab ) > INVLIM )
  {
    t = tx + ( vb - av ) / ab ;
  }
  else
  {
    t = tx + fabs ( ap - pb ) / av * 3600.0 ;
  }
  
  if ( t < 0.0 || tx < 0.0 )
  {
    sw = true ;
#ifdef DEBUG
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "pbx Fault: t, tx = %.2f/%.2f", t , tx ) << endl ;
#endif
    return ;
  }
#ifdef DEBUG
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "pbx return: t, tx = %.2f/%.2f", t , tx ) << endl ;
#endif
}



// program to calculate motion of constantly accelerating train
void
train :: cavt
( double v ,
  double a ,
  double lg ,
  double & t ,
  double & vr ,
  bool & sw )
{
  // ** t: t*t+(2*v/a?)*t-(7200.0*lg/a?)=0, t>0, minimum t.
  double za ;
  const double z = 0.0 ;

  if ( a <= z + INVLIM && a >= z - INVLIM )
  {
    vr = v ;
    t = lg * 3600.0 / v ;
    sw = false ;
  }
  else
  {
    za = v * v + 7200.0 * a * lg ;
    if ( za < z )
    {
      sw = true ;
      return ;
    }
    else
    {
      sw = false ;
    }
    t = ( - v + sqrt ( za ) ) / a ;
    vr = v + a * t ;
  }
}




// ブレーキ終端からの逆算
void
train :: brake
( double avv ,
  double app ,
  nextsta :: gc_size_type k ,
  double ap ,
  nextsta :: gc_size_type j ,
  double & vp ,
  double & bt )
{
  double p , q , c , ab , x ;
  nextsta :: gc_size_type i ;

  vp = avv ;
  const double gconst = 9.80665*36E-4 ;
  bt = 0.0 ;

  for ( i = k ; int ( i ) * direc () >= int ( j ) * direc () ;
        i -= direc () )
  {
    if ( i == j ) q = ap ;
    else	    q = nxx -> spoint ( i ) ;
    if ( i == k ) p = app ;
    else	    p = nxx -> epoint ( i ) ;

    x = direc () * ( p - q ) ;
    ab = abh + egrad ( i ) * gconst / ( 1.0 + eqrot () ) ;
    c = vp * vp + 7200.0 * ab * x ;
    c = ( - vp + sqrt ( c ) ) / ab ;
    bt += c ;
    vp += ab * c ;
  }
}
