// This may look like C code, but it is really -*- C++ -*-
// RTSS --- Railway Total System Simulator
// (c) TAKAGI Ryo (at Kogakuin University)
// ontime.cc --- on-time notchoff position calculator
// -----
// ChangeLog:
// 2009. 11. 27
//  Removed keyword "register".
// 2007. 11. 21
//  Redirected log messages to l_file.
// -----

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

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

//#define DEBUG
//#define TIMEPRINT

#define SOKUDOIHAN 0.00

#define ONTIMEDIFF 1.0



// ノッチオフ地点決定関数
double
train :: ontime
( double np ,
  nextsta :: gc_size_type nj ,
  double nv ,
  double nt )
{
  nxx -> adjpgrad ( nj , np ) ;
  double p = np ;
  double v = nv ;
  double t = nt ;
  nextsta :: gc_size_type j = nj ;
  nextsta :: gc_size_type k ;
  double nxarp , nxarv ;
  if ( ontvlim ( p , j , v , nxarp , k , nxarv ) )
  {
    ontvlc ( np , nj , nv , nt , p , j , v , t , nxarp , k , nxarv ) ;
  }
  if ( t - nt > nxx -> tarrive ( pfirst ) * 2.0 )
  {
    return t ;
  }

  double px = p ;
  double vx = v ;
  double tx = t ;
  np = p ;
  nv = v ;
  nj = j ;
  double tt = t ;
  while ( true )
  {
    if ( ontvlim ( px , nj , vx , nxarp , k , nxarv ) )
    {
      // 制限→定速運転
      ontvlc ( np , nj , nv , tt , p , j , v , t , nxarp , k , nxarv ) ;
      if ( t - nt > nxx -> tarrive ( pfirst ) * 2.0 )
      {
        return t ;
      }
      if ( nj == nxx -> dqgrad () )
      {
        return t ;
      }
      np = p ;
      nv = v ;
      nj = j ;
      tt = t ;
      continue ;
    }
    if ( ontvlim ( p , nj , v , nxarp , k , nxarv ) )
    {
      ontvlc ( px , nj , vx , tx , p , j , v , t , nxarp , k , nxarv , true ) ;
      if ( t - nt > nxx -> tarrive ( pfirst ) * 2.0 )
      {
        return t ;
      }
      if ( nj == nxx -> dqgrad () )
      {
        return t ;
      }
      np = p ;
      nv = v ;
      nj = j ;
      tt = t ;
      continue ;
    }
    np = p ;
    nv = v ;
    nj = j ;
    tt = t ;
    ontcm ( p , j , v , t , px , vx , tx ) ;
    if ( t - nt > nxx -> tarrive ( pfirst ) * 2.0 ) {
      return t ;
    }
    if ( bklen ( px , nj , v , nxx -> varrive () ) * direc ()
         >= direc () * nxx -> parrive () ) 
    {
      return ontbrake ( np , nj , nv , tt ) ;
    }
    if ( bklen ( p , nj , v , nxx -> varrive () ) * direc ()
         >= direc () * nxx -> parrive () )
    {
      return ontbc ( np , nj , nv , tt ) ;
    }
    if ( nj == nxx -> dqgrad () )
    {
      return t ;
    }
  }
}




// 定時ノッチオフ制御ルーチン（精度向上型）
double
train :: ontime_prec
( double np ,
  nextsta :: gc_size_type jgrad ,
  double nv ,
  double nt )
{
  double ntt = nt ;
  car_stat csx = Coast ;
  double brvx = 0.0 ;	// -Wall warning 回避のため初期化
  bool sw_coast_after_brake = true ;	// 最後のブレーキング → false
  int xcount = 0 ;
  while ( xcount < 20000 )
  {
    ++ xcount ;

    // 終了判定と次状態設定
    if ( np * direc () > nxx -> parrive () * direc () - DINVLIM )
    {
      // 終点到達判別
      if ( nv > nxx -> varrive () )
      {
        // 飛び出し抑制
        np = nxx -> parrive () - direc () * INVLIM ;
      }
      else
      {
        // Station_pass , Pass_pass タイプの計算終了条件
        return nt ;
      }
    }
    if ( csx != Brake && nv < 0 )
    {
      // 列車が逆行！ -> 長め時間を返す
      return nt + nxx -> tarrive ( pfirst ) * 10 ;
    }
    if ( nt - ntt >= nxx -> tarrive () * 2 )
    {
      // 時間かかり過ぎ -> 長め時間を返す
      return nt + nxx -> tarrive ( pfirst ) * 10 ;
    }

    // 速度制限探し
    nextsta :: gc_size_type i = 0 ;
    for ( i = nxx -> dqgrad () ;
          int ( i ) * direc () > int ( jgrad ) * direc () ; i -= direc () )
    {
      if ( nxx -> vlimit ( i ) < INVLIM
           || nxx -> vlimit ( i ) >= nxx -> vlimit ( jgrad )
           && nxx -> vlimit ( jgrad ) >= INVLIM || csx != Coast )
      {
        // 制限無効 → 何もしない
        continue ;
      }
      if ( bklen ( np , jgrad , nv , nxx -> vlimit ( i ) ) * direc ()
           >= nxx -> spoint ( i ) * direc () )
      {
        // brake 転向
        csx = Brake ;
        brvx = nxx -> vlimit ( i ) ;
        sw_coast_after_brake = true ;	
        break ;
      }
    }
    if ( csx == Coast
         && bklen ( np , jgrad , nv , nxx -> varrive () ) * direc ()
         >= nxx -> parrive () * direc () )
    {
      // brake 転向条件
      csx = Brake ;
      sw_coast_after_brake = false ;
      brvx = nxx -> varrive () ;
    }
    else if ( csx == Brake && nv <= brvx )
    {
      // brake 終了条件
      if ( sw_coast_after_brake )
      {
        csx = Coast ;
      }
      else
      {
        // このケースは計算終了でもある
        return nt ;
      }
    }

    // 次位置演算
    nt += deltah ;	// increments time
    if ( csx == Coast && nv >= nxx -> vlimit ( jgrad )
         && nxx -> vlimit ( jgrad ) >= INVLIM
         && tresist ( nv , jgrad ) < 0.0 )
    {
      // 定速走行条件
      nposc ( np , jgrad , nv ) ;
    }
    else if ( csx == Coast )
    {
      // 惰行
      npos ( np , jgrad , nv ) ;
    }
    else
    {
      // ブレーキ
      nposb ( np , jgrad , nv ) ;
    }
  }
  if ( xcount >= 20000 )
  {
    cerr << StrPrintf ( "np, jgrad, nv, direc = %10.4f %d %8.2f %4d" ,
                        np , jgrad , nv , direc () ) << endl ;
    exit ( 37 ) ;
  }
  exit ( 38 ) ;	// なぞの事態発生
  return 0.0 ;	// 返らないからよいことにする -Wall warning 回避
}



// ノッチオフ後の走行時間の推定
double
train :: ontimesub
( double np ,
  int jgrad ,
  double nv ,
  double nt )
{
  double ret_val = 0.0 ;
  double compval ;
  if ( ! ontprec )
  {
    ret_val = ontime ( np , jgrad , nv , nt ) ;
    compval = nxx -> tarrive ( pfirst ) + ONTIMEDIFF + tdept - taudx ;
#ifdef TIMEPRINT
    l_ofs << "car " << carno << ", "
        << StrPrintf ( "ret_val, compval = %.2f %.2f" ,
                         ret_val , compval ) << endl ;
#endif
    if ( ret_val < compval )
    {
      ontprec = true ;
    }
  }
  if ( ontprec )
  {
    ret_val = ontime_prec ( np , jgrad , nv , nt ) ;
  }
#ifdef TIMEPRINT
  l_ofs << "car " << carno << ", "
        << StrPrintf ( "prec, ret_val = %.2f" , ret_val ) << endl ;
#endif
  return ret_val ;
}
