// This may look like C code, but it is really -*- C++ -*-
// RTSS --- Railway Total System Simulator
// (c) TAKAGI Ryo (at Kogakuin University)
// sim_ctla.cc --- Realtime substation voltage control routine, type A
// -----
// ChangeLog:
// 2007. 11. 21
//  Redirected log messages to l_file.
// -----

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

#include "kidenrt.hh"
#include "StrPrintf.hh"

using std :: cerr ;
using std :: endl ;


#define DELTA_C_LIM TYPEA_DCLIM
#define DELTA_C_MAX TYPEA_DCMAX

#define DEBUG_TYPEA

// #ifdef DEBUG_TYPEA
// bool sw_x = false ;
// #endif /* DEBUG_TYPEA */



// 回路計算まわりのサブルーチン、リアルタイム制御タイプA
void
feeder :: simcc_ssctl_a
( int & cctt )
{
  // 第1段階……電圧最高のケースを演算
  resetsubcntl () ;	// 全変電所の subcntl を 1 にリセット
  simcccore ( cctt ) ;
  int ipc ;
  double xregr ;
  double regr_old ;
  if ( cctt )
  {
    // 計算正常終了のとき
    simcc_ssctl_a_regipc ( ipc , xregr ) ;	// 回生絞込み率チェック
    if ( ipc < 0 ||
	 fabs ( xregr - 1.0 ) < INVLIM )
    {
      // 絞り込みの問題なし
#ifdef DEBUG_TYPEA
      l_ofs << "No problem regeneration. return." << endl ;
#endif
      return ;
    }
  }
  else
  {
    // 計算異常終了のとき……ここでは単にリターン
    l_ofs << "ccal failed!" << endl ;
    return ;
  }
  double subcntl_pre , dv_dsubcntl , subcntl_ppx = 0.0 ;
  int ipc_successive = 0 ;
  for ( int k = 0 ; k < TYPEA_REGMAX ; ++ k )
  {
#ifdef DEBUG_TYPEA
    l_ofs << StrPrintf ( "regeneration check , cycle %d, suc %d" ,
                         k , ipc_successive ) << endl ;
#endif
    int ipcx = ipc ;
    regr_old = xregr ;
    double regr_b = getCAR ( ipc ) . regenrate () ;
    subcntl_pre = getSUB ( 0 ) . getsubcntl () ;	// 全部同じ→1つ見ればよい
    if ( k == 0 )
      subcntl_ppx = subcntl_pre ;	// 初めてですから
    bool sw_subc_shippai = false ;
    if ( subcntl_pre < DELTA_C_LIM )
    {
      // 下限→電圧保証 phase へ
      dv_dsubcntl = 0.0 ;
#ifdef DEBUG_TYPEA
      l_ofs << "already minimized. skip" << endl ;
#endif
      continue ;
    }
    double delta_c_div = 1.0 ;
    simcc_ssctl_a_dreg
      ( cctt , delta_c_div , dv_dsubcntl , subcntl_pre , regr_b , ipc ) ;
    if ( ! cctt )
    {
      // 計算異常終了のとき・やめる
      simcc_ssctl_a_setctl ( subcntl_pre ) ;
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      if ( k == 0 ) return ;
      else
      {
	sw_subc_shippai = true ;
	break ;
      }
    }
    if ( sw_subc_shippai )
    {
#ifdef DEBUG_TYPEA
      l_ofs << "subc shippai. proceed on to voltage phase." << endl ;
#endif
      break ;		// 計算失敗……回生優先モードから抜ける
    }
    // 微分から制御値の計算
    double dv_dcntl_sum = dv_dsubcntl ;
    if ( fabs ( dv_dcntl_sum ) < INVLIM )
    {
      // 電圧低下で改善なし
#ifdef DEBUG_TYPEA
      l_ofs << "No need to do further regeneration phase." << endl ;
#endif
      simcc_ssctl_a_setctl ( subcntl_ppx ) ;	// 元に戻る
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      if ( k == 0 ) return ;
      else break ;
    }
    dv_dcntl_sum = ( regr_b - 1.0 ) / dv_dcntl_sum ;
#ifdef DEBUG_TYPEA
    l_ofs << StrPrintf ( "dv_dcntl_sum = %12.6f" , dv_dcntl_sum ) << endl ;
#endif
    // 制御値の制限
    double xdx = dv_dcntl_sum * dv_dsubcntl ;
    if ( fabs ( xdx ) > DELTA_C_MAX + INVLIM )
    {
      dv_dcntl_sum *= DELTA_C_MAX ;
      dv_dcntl_sum /= fabs ( xdx ) ;
#ifdef DEBUG_TYPEA
      l_ofs << StrPrintf ( "dv_dcntl_sum: set to %12.6f" , dv_dcntl_sum )
            << endl ;
#endif
    }
    bool sw_first = true ;
    bool sw_eliminated = false ;
    bool sw_successive = false ;
    while ( fabs ( dv_dcntl_sum ) > DELTA_C_LIM || sw_first )
    {
      sw_first = false ;
      simcc_ssctl_a_addctl ( dv_dcntl_sum * dv_dsubcntl ) ;
#ifdef DEBUG_TYPEA
      l_ofs << StrPrintf ( "+ ( %12.6f ) = %12.6f" ,
                           dv_dcntl_sum * dv_dsubcntl ,
                           getSUB ( 0 ) . getsubcntl () )
            << endl ;
#endif
      simcccore ( cctt ) ;		// 回路計算
      ipc = -10 ;
      xregr = 1.0 ;
      if ( cctt )
      {			// 計算正常終了のとき
	simcc_ssctl_a_regipc ( ipc , xregr ) ;	// ipc の発見
	if ( ipc < 0 || fabs ( xregr - 1.0 ) < INVLIM )
	{
#ifdef DEBUG_TYPEA
	  l_ofs << "problem regenation eliminated." << endl ;
#endif
	  sw_eliminated = true ;
	  break ;		// 絞り込みの問題なし
	}
	if ( ipc == ipcx && xregr - regr_old < TYPEA_REGR_D )
	{
	  if ( ipc_successive == 0 )
	  {
	    // 繰返し回数上限突破
	    subcntl_ppx = subcntl_pre ;	// 時に戻る場所を保存
	  }
	  ++ ipc_successive ;
	  if ( ipc_successive == TYPEA_IPCMAX )
	  {
	    // 元に戻す
#ifdef DEBUG_TYPEA
	    l_ofs << "No effect of voltage reduction." << endl ;
#endif
	    simcc_ssctl_a_setctl ( subcntl_ppx ) ;
	    simcccore ( cctt ) ;	// 計算失敗はしないはず
	    if ( k == 0 )
	      return ;
	    else if ( k - ipc_successive == - 1 )
	      return ;
	    else
	    {
	      sw_successive = true ;
	      break ;
	    }
	  }
	}
	else
	{
	  ipc_successive = 0 ;
	  subcntl_ppx = subcntl_pre ;
	}
	break ;
      }
      else
      {
	// 計算異常終了のとき……元に戻して
#ifdef DEBUG_TYPEA
	l_ofs << "calculation failed in voltage reduction. try again."
              << endl ;
#endif
	simcc_ssctl_a_setctl ( subcntl_ppx ) ;
	dv_dcntl_sum /= 2.0 ;
      }
    }
    if ( sw_eliminated || sw_successive )
      break ;
    if (  ( ! cctt ) || fabs ( dv_dcntl_sum ) <= DELTA_C_LIM )
    {
#ifdef DEBUG_TYPEA
      l_ofs << "shippai in voltage reduction." << endl ;
#endif
      simcc_ssctl_a_setctl ( subcntl_ppx ) ;
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      if ( k == 0 ) return ;
      else break ;
    }
  }
  // 最低電圧車両 ipc の発見。見つからなかったら終了
  double xvolt ;
  simcc_ssctl_a_volipc ( ipc , xvolt ) ;
  if ( ipc < 0 || xvolt - TYPEA_MINVOLT < 0.0 )
  {
#ifdef DEBUG_TYPEA
    l_ofs << "No low-voltage cars. return." << endl ;
#endif
    return ;		// 最低電圧の問題なし
  }
  for ( int k = 0 ; k < TYPEA_TRNMAX ; ++ k )
  {
    double volt_b = getCAR ( ipc ) . volt () ;
    subcntl_pre = getSUB ( 0 ) . getsubcntl () ;
    if ( subcntl_pre > 1.0 - INVLIM )
    {
      // 上限→次へ
#ifdef DEBUG_TYPEA
      l_ofs << "already maximized. skip." << endl ;
#endif
      dv_dsubcntl = 0.0 ;
      continue ;
    }
    simcc_ssctl_a_setctl ( 1.0 ) ;	// 電圧を限度まであげる
    simcccore ( cctt ) ;		// 回路計算
    if ( cctt )
    {
      // 計算正常終了→微分を求める
      double volt_a = getCAR ( ipc ) . volt () ;
      volt_a -= volt_b ;
      dv_dsubcntl = volt_a / ( 1.0 - subcntl_pre ) ;
      simcc_ssctl_a_setctl ( subcntl_pre ) ;	// 戻す
#ifdef DEBUG_TYPEA
      l_ofs << StrPrintf ( "normally calculated, dv_dsub = %10.5f" ,
                           dv_dsubcntl ) << endl ;
#endif
    }
    else
    {
      // 計算異常終了→やめる
#ifdef DEBUG_TYPEA
      l_ofs << "calculation failed." << endl ;
#endif
      simcc_ssctl_a_setctl ( subcntl_pre ) ;
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      return ;
    }
    double dv_dcntl_sum = dv_dsubcntl ;
    if ( fabs ( dv_dcntl_sum ) < INVLIM )
    {
      // 電圧上昇で改善なし
      simcc_ssctl_a_setctl ( subcntl_pre ) ;	// 元に戻す
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      return ;
    }
    dv_dcntl_sum /= volt_b - TYPEA_MINVOLT ;
    simcc_ssctl_a_addctl ( dv_dcntl_sum * dv_dsubcntl ) ;
    simcccore ( cctt ) ;	// 回路計算
    if ( cctt )
    {
      // 計算正常終了のとき
      simcc_ssctl_a_volipc ( ipc , xvolt ) ;		// ipc の発見
      if ( ipc < 0 || xvolt - TYPEA_MINVOLT < 0.0 )
	return ;		// 電圧の問題なし
    }
    else
    {
      // 計算異常終了のとき……元に戻して次へ
      simcc_ssctl_a_setctl ( subcntl_pre ) ;
      simcccore ( cctt ) ;	// 計算失敗はしないはず
      return ;
    }
  }
}



// simcc_ssctl_a のサブルーチン，変電所制御の回生率微分値の計算
void
feeder :: simcc_ssctl_a_dreg
( int & cctt ,
  double delta_c_div ,
  double & dv_dsubcntl ,
  double & subcntl_pre ,
  double regr_b ,
  int ipc )
{
  // 各変電所毎に電圧を限度まで下げる
  bool subcntl_zero_one = false ;
  if ( subcntl_pre < TYPEA_DELTA_C / delta_c_div )
  {
    subcntl_zero_one = true ;
    simcc_ssctl_a_setctl ( 0.0 ) ;
#ifdef DEBUG_TYPEA
    l_ofs << "minimized." << endl ;
#endif
  }
  else
  {
    simcc_ssctl_a_setctl ( subcntl_pre - TYPEA_DELTA_C / delta_c_div ) ;
#ifdef DEBUG_TYPEA
    l_ofs << StrPrintf ( "minus %.5f" , TYPEA_DELTA_C / delta_c_div )
          << endl ;
#endif
  }
  simcccore ( cctt ) ;		// 回路計算
  if ( cctt )
  {
    // 計算正常終了→微分を求める
    double regr_a = getCAR ( ipc ) . regenrate () ;
    regr_a -= regr_b ;
    if ( subcntl_zero_one )
      dv_dsubcntl = regr_a / subcntl_pre ;
    else
      dv_dsubcntl = regr_a * delta_c_div / TYPEA_DELTA_C ;
    simcc_ssctl_a_setctl ( subcntl_pre ) ;	// 戻す
#ifdef DEBUG_TYPEA
    l_ofs << StrPrintf ( "normally calculated, dv_dsub = %10.5f" ,
                         dv_dsubcntl ) << endl ;
#endif
    return ;
  }
  else
  {
    // 計算異常終了・TYPEA_DELTA_C 半分にして再トライ
    if ( TYPEA_DELTA_C / ( delta_c_div * 2.0 ) < DELTA_C_LIM )
    {
#ifdef DEBUG_TYPEA
      l_ofs << "calculation failed." << endl ;
#endif
      return ;
    }
    else
    {
#ifdef DEBUG_TYPEA
      l_ofs << "calculation failed. try again." << endl ;
#endif
      simcc_ssctl_a_dreg ( cctt , delta_c_div * 2.0 , dv_dsubcntl ,
			   subcntl_pre , regr_b , ipc ) ;
      return ;
    }
  }
}



// simcc_ssctl_a のサブルーチン，列車回生率チェック
void
feeder :: simcc_ssctl_a_regipc
( int & ipc ,
  double & xregr )
{
  ipc = -10 ;
  xregr = 1.0 ;
  for ( int i = 0 ; i < ncar ; ++ i )
  {
    if ( getCAR ( i ) . regendead () )
      continue ;	// 回生失効時は考慮しない
    if ( getCAR ( i ) . rnotchvl () > - INVLIM )
      continue ;	// フルノッチ比負のケースのみ考慮
    double yregr = getCAR ( i ) . regenrate () ;
    if ( yregr < xregr )
    {
      xregr = yregr ;
      ipc = i ;
    }
  }
#ifdef DEBUG_TYPEA
  l_ofs << StrPrintf ( "ipc search ended, No.%3i: regenrate %6.3f" ,
                       ipc , xregr ) << endl ;
#endif
}



// タイプA, setsubcntl を行う
void
feeder :: simcc_ssctl_a_setctl
( double ppx )
{
  for ( int i = 0 ; i < nss ; ++ i )
  {
    getSUB ( i ) . setsubcntl ( ppx ) ;
  }
}



// タイプA, addtosubcntl を行う
void
feeder :: simcc_ssctl_a_addctl
( double ppx )
{
  for ( int i = 0 ; i < nss ; ++ i )
  {
    getSUB ( i ) . addtosubcntl ( ppx ) ;
  }
}



// simcc_ssctl_a のサブルーチン，列車パンタ点電圧チェック
void
feeder :: simcc_ssctl_a_volipc
( int & ipc ,
  double & xvolt )
{
  xvolt = TYPEA_MINVOLT ;
  ipc = - 10 ;
  for ( int i = 0 ; i < ncar ; ++ i )
  {
    if ( getCAR ( i ) . rnotchvl () < INVLIM )
      continue ;	// フルノッチ比正のケースのみ考慮
    double yvolt = getCAR ( i ) . volt () ;
    if ( yvolt < xvolt )
    {
      xvolt = yvolt ;
      ipc = i ;
    }
  }
}
