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


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

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

//#define DEBUG

// ----------------------------------
// globvar.hh で定義する const 変数群
// ----------------------------------

// ccal () のリミットサイクル発見ルーチンは FAILNUM 回の接近で発見する.
// Default = 3
#define FAILNUM gc_ccalfail_failnum

// ccal () のリミットサイクル閾値. Default = 1.0
#define FAILDIF gc_ccalfail_faildif

// リミットサイクル発見後は修正量を FAILDIV 倍に制限. Default = 1.0
#define FAILDIV gc_ccalfail_faildiv

// 最大 CIRCULATION 回回路計算を実行. Default = 250
#define CIRCULATION gc_ccalfail_circ

// 最大 CIRCULATION 回回路計算を実行. Default = 0.1
#define TERMINATION_CRITERIA gc_ccal_termination

// Y 行列演算ルーチン……独立させる
void
feeder :: ycal
()
{
  feedy . renew () ;

  // 列車のデータを feedline 行列にたたき込む
  for ( int i = 0 ; i < feeders ; ++ i )
    getFLN ( i ) . clearcar () ;
  for ( int i = 0 ; i < ncar ; ++ i )
    getFLN ( getCAR ( i ) . nfline () ) += getCAR ( i ) . feep () ;

  // 距離ソート、ブランチインピーダンス計算
  for ( int i = 0 ; i < feeders ; ++ i )
    feedy += getFLN ( i ) . sortx () ;

  // Ｙ行列の計算  
  feedy . y ( ncss , ymtx ) ;

  // Ｙ行列の４分割と行列の書き直し
  static matrix aa , ab , ba , bb , bb_inv ;
  ymtx . quaddiv ( nss , nss , aa , ab , ba , bb ) ;
//   bb . inverse () ;
//   aa -= ab * bb * ba ;
//   ab *= bb ;
//   ba = - bb * ba ;
//   ymtx . quadjoin ( aa , ab , ba , bb ) ;
  int det = bb . decompose_LU () ;
  if ( det == 0 )
  {
    l_ofs << "THERE IS SOMETHING WRONG HERE..." << endl ;
  }
  bb . inverse ( bb_inv ) ;
  aa -= ab * bb_inv * ba ;
  ab *= bb_inv ;
  bb = ba ;
  ba = bb_inv ;
  ba *= - 1.0 ;
  ba *= bb ;
  ymtx . quadjoin ( aa , ab , ba , bb_inv ) ;
}


// 饋電等価回路演算ルーチン
int
feeder :: ccal
()
{
  int det_xx ;
  int ncirc ;
  int lims = 0 ;
  int i ;
  int j ;
  fvec . renewmn ( ncss ) ;
  int ccm = 0 ;	// number of trials. First try = 0
  static bool ccbfzz = true ;
  static double * * ccbftt = new double * [ CIRCULATION ] ;
  if ( ccbfzz ) {
    for ( i = 0 ; i < CIRCULATION ; ++ i )
    {
      ccbftt [ i ] = new double [ ncss ] ;  // ccbftt 配列の領域確保
    }
    ccbfzz = false ;				// これは初回のみ実行
  }

 NEWTON:
  // ニュートン・ラプソン法
  double ff [ CIRCULATION ] ;
  short int limcyc = 0 ;
  // settetazero () ;
  for ( ncirc = 0 ; ncirc < CIRCULATION ; ++ ncirc )
  {
    notchrate () ;
    ff [ ncirc ] = ffunc ( fvec ) ;
    l_ofs << StrPrintf ( "ff[%i] = %.2f" , ncirc , ff [ ncirc ] ) << endl ;
    if ( ff [ ncirc ] <= TERMINATION_CRITERIA )
    {
      for ( i = 0 ; i < ncss ; ++ i )
      {
	getCSD ( i ) -> teta () += fvec ( i ) / 2.0 ;
	getCSD ( i ) -> teta_vi () ;
      }
      return 1 ;
    }

    for ( i = 0 ; i < ncss ; ++ i )		// teta 変数の現在値を記録
      ccbftt [ ncirc ] [ i ] = getCSD ( i ) -> teta () ;

    // 収束しそうな回路演算を途中で止めないために
    if ( limcyc < FAILNUM &&     		// リミットサイクルの発見
	 ncirc > 0 &&				// 収束に向かっているなら
	 ff [ ncirc - 1 ] < ff [ ncirc ] )	// チェックしない
    {
      for ( j = lims ; j < ncirc ; ++ j )
      {
	if ( ( ccm != 3 ) && fabs ( ff [ j ] - ff [ ncirc ] ) < FAILDIF
	     || ( ccm == 3 )
	     && fabs ( ff [ j ] - ff [ ncirc ] ) < FAILDIF * FAILDIV )
	{
	  ++ limcyc ;
	  lims = j ;
	  break ;
	}
      }
    }
    if ( limcyc == FAILNUM || ncirc == CIRCULATION - 1 )
    {
      // リミットサイクル発見、処理を行う
      switch ( ccm )
      {
      case 0 :
      case 2 :
        // 1度目か3度目: １サイクル分の平均をとる
        for ( j = 0 ; j < ncss ; ++ j )
        {
          for ( int k = lims + 1 ; k < ncirc ; ++ k )
            getCSD ( j ) -> teta () += ccbftt [ k ] [ j ] ;
          getCSD ( j ) -> teta () /= ncirc - lims ;
        }
        std :: cerr << "mean value of 1 cycle -> teta" << std :: endl ;
        break ;

      case 1 :
        // 2度目: 最初からやり直し
        restorevi () ;
        std :: cerr << "restart" << std :: endl ;
        break ;

      case 3 :
      default :
        // 4度目: 計算失敗、実行終了
        return 0 ;
      }

      // ループを抜ける
      break ;
    }

    gradf () ;		// 増分値の計算
    det_xx = gff_xx . decompose_LU () ;
    if ( det_xx == 0 )
    {
      for ( i = 0 ; i < ncss ; ++ i )
	getCSD ( i ) -> teta () += i * 5;
    }
    else
    {
      gff_xx . eqnsolve ( fvec , gamma ) ;
      for ( i = 0 ; i < ncss ; ++ i )
      {
        getCSD ( i ) -> add_to_teta ( - gamma ( i ) , ccm ) ;
      }
    }
  }
  if ( ccm == 3 ) return 0;
  ++ ccm ;
  goto NEWTON ;
}
