package matsubara.carcontroller;

import matsubara.gear.*;


/**
 * 
タイトル: ＴＨＳシミュレータ
 * 
説明: プリウスのハイブリッドシステム（ＴＨＳ）の心臓部、遊星歯車機構のシミュレーション
 * 
著作権: Copyright (c) 2002 m.matsubara 
 * @author m.matsubara
 * @version 1.2.0
 */

public class ThsController extends CarController {
  PlanetaryGear m_planetaryGear;
  boolean m_bGasoline;
  boolean m_bMotor;
  boolean m_bKaisei;
  double m_rBatterySOC = 60.0;      //  ＨＶバッテリー残量（SOC = State of charge）;
  boolean m_bEngineRequest = false; //  ＨＶバッテリーが足りないとき（４０％未満）ＯＮ　→　５０％になるまで
  boolean m_bPowerSaveMode = false; //  いわゆるカメ
  //  THS 動作モード
  /** エネルギーの流れなし */
  public static final int thsStop = 0;
  /** 停止で充電 */
  public static final int thsStopAndEngine = 1;
  /** モーターのみで発進 */
  public static final int thsStart = 2;
  /** 通常運転 */
  public static final int thsNormal = 3;
  /** 通常運転(オーバードライブ・モーターと発電機の役割が逆転) */
  public static final int thsNormalOD = 4;
  /** 全開加速（エンジン+モーター） */
  public static final int thsFullAccel = 5;
  /** 全開加速（エンジン+モーター） */
  public static final int thsFullAccelOD = 6;
  /** 回生ブレーキ */
  public static final int thsKaisei = 7;
  /** 回生ブレーキ＋エンジンは発電機の負荷で動いている */
  public static final int thsKaiseiAndEngine = 8;
  /** モーターのみで発進 */
  public static final int thsBackAndEngine = 9;
  /** 動作モード */
  int m_nThsMode = thsStop;

  /**
   * ThsController の初期化
   * @param nPlanetaryGearX プラネタリギアを描くときのX座標
   * @param nPlanetaryGearY プラネタリギアを描くときのX座標
   * @param nPlanetaryGearSize プラネタリギアを描くときのサイズ（半径）
   */
  public ThsController(int nPlanetaryGearX, int nPlanetaryGearY, int nPlanetaryGearSize) throws EPlanetaryGear {
    super();
    m_planetaryGear = new PlanetaryGear(nPlanetaryGearX, nPlanetaryGearY, nPlanetaryGearSize, 30, 23, 78, 4, 4); //  動力分割機構
    setFuel(50);  //  燃料満タン
  }

  /**
   * シフトポジションの設定（安全装置付き♪）
   * 安全装置（？）が付いているためDレンジからいきなり P や R に入ったりしない。
   * @param sShiftPosition 新しいシフトポジション
   */
  public void setShiftPosition(String sShiftPosition) {
    double rSpeed = getSpeed();
    if (sShiftPosition.equals("P")) {
      if (rSpeed == 0)
        super.setShiftPosition(sShiftPosition);
      else
        super.setShiftPosition("N");
    }
    else if (sShiftPosition.equals("R")) {
      if (rSpeed <= 3)
        super.setShiftPosition(sShiftPosition);
//      else
//        super.setShiftPosition("N");
    }
    else if (sShiftPosition.equals("N"))
      super.setShiftPosition(sShiftPosition);
    else if (sShiftPosition.equals("D") || sShiftPosition.equals("B")) {
      if (rSpeed >= -3)
        super.setShiftPosition(sShiftPosition);
//      else
//        super.setShiftPosition("N");
    }
  }

  /**
   * 車の状態をシミュレートする
   * @param rMilliSec 経過秒数
   * @param nAccel アクセルの踏み込み量(%)
   * @param nBrake ブレーキの踏み込み量(%)
   */
  protected void driveInternal(double rMilliSec, int nAccel, int nBrake) {
    String sShiftPosition = getShiftPosition();
    if (getFuel() <= 0) {
      //  インチキだがガス欠判断
      nAccel = 0;
      nBrake = 30;
    }

    if (sShiftPosition.equals("D"))
      driveInternal_D_B(rMilliSec, nAccel, nBrake);
    else if (sShiftPosition.equals("B"))
      driveInternal_D_B(rMilliSec, nAccel, nBrake);
    else if (sShiftPosition.equals("R"))
      driveInternal_R(rMilliSec, nAccel, nBrake);
    else if (sShiftPosition.equals("P"))
      driveInternal_P(rMilliSec, nAccel, nBrake);
    else
      driveInternal_N(rMilliSec, nAccel, nBrake);     //  N レンジ

    m_bMotor = (m_nThsMode == thsStart) || (m_nThsMode == thsFullAccel) || (m_nThsMode == thsFullAccelOD) || (m_nThsMode == thsBackAndEngine);
    m_bKaisei = (m_nThsMode == thsKaisei) || (m_nThsMode == thsKaiseiAndEngine);
    m_bGasoline = (m_nThsMode == thsFullAccel) || (m_nThsMode == thsFullAccelOD) || (m_nThsMode == thsNormal) || (m_nThsMode == thsNormalOD) || (m_nThsMode == thsStopAndEngine)|| (m_nThsMode == thsBackAndEngine);

    if (m_rBatterySOC > 80.0)
      m_rBatterySOC = 80.0;
    if ((m_bEngineRequest == false) && (m_rBatterySOC < 45)) {
      m_bEngineRequest = true;
      System.out.println("m_bEngineRequest = true");
    }
    if (m_rBatterySOC < 40)
      m_bPowerSaveMode = true;
    if ((m_bEngineRequest != false) && (m_rBatterySOC >= 50)) {
      m_bEngineRequest = false;
      m_bPowerSaveMode = false;
      System.out.println("m_bEngineRequest = false");
    }
  }

  private double engineRpm(double rTargetEngineRpm, double rMilliSec) {
    //  回転数の制御
    double rEngineRpm = m_planetaryGear.getPlaCarrierSpeed();
    if (rEngineRpm < rTargetEngineRpm) {
      rEngineRpm += (rMilliSec / 1000) * 1000;
      if (rEngineRpm > rTargetEngineRpm)
        rEngineRpm = rTargetEngineRpm;
    }
    else if (rEngineRpm > rTargetEngineRpm) {
      rEngineRpm -= (rMilliSec / 1000) * 2000;
      if (rEngineRpm < rTargetEngineRpm)
        rEngineRpm = rTargetEngineRpm;
    }
    return rEngineRpm;
  }

  /**
   * Pレンジの時のシミュレーション
   * @param rMilliSec 経過秒数
   * @param nAccel アクセルの踏み込み量(%)
   * @param nBrake ブレーキの踏み込み量(%)
   */
  private void driveInternal_P(double rMilliSec, int nAccel, int nBrake) {
    m_nThsMode = thsStop;

    //  ＴＨＳ各目標値
    double rTargetEngineRpm = 0;

    //  エンジンの状態
    if (nAccel >= 5) {
      //  エンジン燃焼
      rTargetEngineRpm = 1000 + nAccel * 500 / 100 ;
    }

    //　バッテリーの状態が悪いとき
    if (m_bEngineRequest) {
      if (rTargetEngineRpm < 1250)
        rTargetEngineRpm = 1250;
      rTargetEngineRpm = 1250;
      m_nThsMode = thsStopAndEngine;
      m_rBatterySOC += 0.00010 * rMilliSec;
    }

    //  回転数の制御
    double rEngineRpm = engineRpm(rTargetEngineRpm, rMilliSec);

    m_planetaryGear.setRingGearSpeed(0);
    m_planetaryGear.setPlaCarrierSpeed(rEngineRpm);
    m_planetaryGear.move(rMilliSec / 100);
  }

  /**
   * Rレンジの時のシミュレーション
   * @param rMilliSec 経過秒数
   * @param nAccel アクセルの踏み込み量(%)
   * @param nBrake ブレーキの踏み込み量(%)
   */
  private void driveInternal_R(double rMilliSec, int nAccel, int nBrake) {
    m_nThsMode = thsStop;

    //  ＴＨＳ各現状値
    double rSpeed = getSpeed();

    //  ＴＨＳ各目標値
    double rTargetSpeed = - 5 - (double)(nAccel * 35 / 100);
    double rTargetEngineRpm = 0;
    if (m_bEngineRequest) {
      if (rTargetSpeed < -16)
        rTargetSpeed = -16;
      rTargetEngineRpm = 1200;
    }
    if (m_bPowerSaveMode == false && m_bEngineRequest && rSpeed < -5)  //  エンジン回転要求があっても速度が -5km/h より速いときはエンジンを回さない。
      rTargetEngineRpm = 0;

    double rEngineRpm = engineRpm(rTargetEngineRpm, rMilliSec);

    //  加速・自然減速の計算
    m_bMotor = true;
    m_nThsMode = thsStart;
    if (rSpeed > rTargetSpeed) {
      //  加速の計算
      rSpeed += (rTargetSpeed - rSpeed) * rMilliSec / 1000 * 0.15;
      if (rSpeed < rTargetSpeed)
        rSpeed = rTargetSpeed;
    }
    else {
      rSpeed += 2.20 * rMilliSec / 1000;

      if (rSpeed > rTargetSpeed)
        rSpeed = rTargetSpeed;
      if (rSpeed < -10)
        m_nThsMode = thsKaisei;
    }
    if (rEngineRpm > 1000)
      m_nThsMode = thsBackAndEngine;

    //  ブレーキの判断
    if (isParkingBrake() && nBrake < 50)
      nBrake = 50;  //  パーキングブレーキを普通のブレーキの50%とみなす。
    if (nBrake > 5) {
      rSpeed += 0.25 * nBrake * rMilliSec / 1000;
      if (rSpeed > 0)
        rSpeed = 0;
      if (rSpeed < -10)
        m_nThsMode = thsKaisei;
      else
        m_nThsMode = thsStop;
    }

    //  エンジンが回転していて速度が０の時
    if (rEngineRpm > 1000 && rSpeed == 0)
      m_nThsMode = thsStopAndEngine;

    if (m_nThsMode == thsStart)
      m_rBatterySOC -= rSpeed * -0.0000030 * rMilliSec;
    else if (m_nThsMode == thsKaisei)
      m_rBatterySOC += rSpeed * -0.0000010 * rMilliSec;
    else if (m_nThsMode == thsBackAndEngine)
      m_rBatterySOC += 0.0000020 * rMilliSec;
    else if (m_nThsMode == thsStopAndEngine)
      m_rBatterySOC += 0.00010 * rMilliSec;

//    double rTyreDiameter = 165 * 0.65 * 2 + 15 * 25.4;
    double rTyreDiameter = 285 * 2;
    double rTyreGirth = rTyreDiameter * Math.PI;
    double rRingGear = (3.927 / rTyreDiameter / Math.PI / 60 * 1000 * 1000) * rSpeed;

    m_planetaryGear.setRingGearSpeed(rRingGear);
    m_planetaryGear.setPlaCarrierSpeed(rEngineRpm);
    m_planetaryGear.move(rMilliSec / 100);
  }

  /**
   * Nレンジの時のシミュレーション
   * @param rMilliSec 経過秒数
   * @param nAccel アクセルの踏み込み量(%)
   * @param nBrake ブレーキの踏み込み量(%)
   */
  private void driveInternal_N(double rMilliSec, int nAccel, int nBrake) {
    double rSpeed = getSpeed();
    //  Ｎレンジでは回生ブレーキが使えないのでブレーキの利きがやや緩い
    if  (rSpeed > 0) {
      rSpeed -= (1.4 + rSpeed / 100 * 1.5) * rMilliSec / 1000 * 0.5;      //  自然減速分
      rSpeed -= (0.20 + rSpeed / 100 * 0.8) * nBrake * rMilliSec / 1000 * 0.5;  //  ブレーキ
      if (rSpeed < 0)
        rSpeed = 0;
    }
    else if (rSpeed < 0) {
      rSpeed += (1.4 - rSpeed / 100 * 1.5) * rMilliSec / 1000 * 0.5;      //  自然減速分
      rSpeed += (0.20 - rSpeed / 100 * 0.8) * nBrake * rMilliSec / 1000 * 0.5;  //  ブレーキ
      if (rSpeed > 0)
        rSpeed = 0;
    }

//    double rTyreDiameter = 165 * 0.65 * 2 + 15 * 25.4;
    double rTyreDiameter = 285 * 2;
    double rTyreGirth = rTyreDiameter * Math.PI;
    double rRingGear = (3.927 / rTyreDiameter / Math.PI / 60 * 1000 * 1000) * rSpeed;

    m_planetaryGear.setRingGearSpeed(rRingGear);
    m_planetaryGear.setPlaCarrierSpeed(0);
    m_planetaryGear.move(rMilliSec / 100);
    m_nThsMode = thsStop;
  }

  /**
   * DレンジとBレンジの時のシミュレーション
   * @param rMilliSec 経過秒数
   * @param nAccel アクセルの踏み込み量(%)
   * @param nBrake ブレーキの踏み込み量(%)
   */
  private void driveInternal_D_B(double rMilliSec, int nAccel, int nBrake) {
    //  ＴＨＳ各現状値
    double rSpeed = getSpeed();
    double rEngineRpm = m_planetaryGear.getPlaCarrierSpeed();

    //  ＴＨＳ各目標値
    double rTargetSpeed = 13 + nAccel * 1.6;
    double rTargetEngineRpm = 0;

    //  エンジンの状態
//    double rEngineStopSpeed = (m_rBatterySOC >= 45) ? 45 : 40;    //  エンジン停止速度（エンジンとモーターの速度の境界）
    double rEngineStopSpeed = 45;
    double rMoterAccelDiff = (m_rBatterySOC >= 50) ? 20 : 10;  //  モーターのみでの加速のしやすさ（値の大きい方がモーターのみで加速しやすい）
    if ((rSpeed >= rEngineStopSpeed || rSpeed < rTargetSpeed - rMoterAccelDiff) && nAccel >= 10) {
      //  エンジン燃焼
      rTargetEngineRpm = 1150 + (rTargetSpeed - 40) * (rTargetSpeed - 40) * 2850 / 10000 ;
      if (rTargetEngineRpm > 4000)
        rTargetEngineRpm = 4000;
      if (rEngineRpm >= 1000)
        m_nThsMode = thsNormal;
      else
        m_nThsMode = thsStart;
    }
    else if (rSpeed >= 3 && rSpeed < 15 && rEngineRpm >= 1000) {
      rTargetEngineRpm = 1150;
      m_nThsMode = thsNormal;
    }
    else {
      //  エンジンは発電機によって回されているか、または止まっている（燃焼していない）
      rTargetEngineRpm = 1000 + (rSpeed - 50) * 10;
      if (rSpeed < 45)
        rTargetEngineRpm = 0;
      m_nThsMode = thsStart;
    }

    //  ＨＶバッテリーＳＯＣが少ないとき
    //  減速中（ブレーキを踏んでいて、なおかつ速度が１５ｋｍ／ｈ以上でないとき）は、エンジンを止めることがある
    if (m_bPowerSaveMode) {
      if (nBrake <= 5 || rSpeed < 15) {
        rTargetEngineRpm += 100;
        rTargetEngineRpm = rTargetEngineRpm > 1250 ? rTargetEngineRpm : 1250;
        rTargetEngineRpm = rTargetEngineRpm < 4000 ? rTargetEngineRpm : 4000;
      }
      if (rSpeed != 0)
        m_nThsMode = thsNormal;
      if (rTargetSpeed >= 140)
        rTargetSpeed = 140;
    }

    rEngineRpm = engineRpm(rTargetEngineRpm, rMilliSec);

    //  加速・自然減速の計算
    if (rSpeed < rTargetSpeed) {
      //  加速の計算
      double rMaxPowerRatio = 0.5 + (160 - rSpeed) / 320;
      if (m_bPowerSaveMode)
        rMaxPowerRatio *= 0.9;  //  カメモードの時は加速は鈍くする
      if (getShiftPosition().equals("D"))
        rSpeed += Math.pow(rTargetSpeed - rSpeed, 0.9) * rMilliSec / 1000 * 0.120 * rMaxPowerRatio; //  D レンジの加速
      else
        rSpeed += Math.pow(rTargetSpeed - rSpeed, 0.9) * rMilliSec / 1000 * 0.111 * rMaxPowerRatio; //  B レンジの加速
      if (rSpeed > rTargetSpeed)
        rSpeed = rTargetSpeed;
      if ((rSpeed < rTargetSpeed - 15) || (rTargetSpeed > 140) || (rEngineRpm == 0)) {
        if (rEngineRpm >= 1000) {
          if (m_bPowerSaveMode == false)
            m_nThsMode = thsFullAccel;
        }
        else
          m_nThsMode = thsStart;  //　微妙な加速
      }
    }
    else {
      //  自然減速
      double rBrakeRatio = 1.0; //  Ｄレンジのときの自然減速の割合との比較
      if (getShiftPosition().equals("B"))
        rBrakeRatio = 1.4;  //  Ｂレンジの比率
      rSpeed -= (1.4 + rSpeed / 100 * 1.5) * rMilliSec / 1000 * rBrakeRatio;
      if (rSpeed < rTargetSpeed)
        rSpeed = rTargetSpeed;
      if ((nAccel == 0) && (rSpeed > 15))
          m_nThsMode = thsKaisei;
      m_nThsMode = thsKaisei;
    }

    //  ブレーキの判断
    if (isParkingBrake() && nBrake < 50)
      nBrake = 50;  //  パーキングブレーキを普通のブレーキの50%とみなす。
    if (nBrake > 5) {
      rSpeed -= (0.20 + rSpeed / 100 * 0.8) * nBrake * rMilliSec / 1000;
      if (rSpeed < 0)
        rSpeed = 0;
      if (rSpeed > 10)
        m_nThsMode = thsKaisei;
      else
        m_nThsMode = thsStop;
    }

    if ((m_nThsMode == thsKaisei) && (rTargetEngineRpm > 0))
      m_nThsMode = thsKaiseiAndEngine;
    if ((m_nThsMode == thsStop) && rEngineRpm >= 1250)
      m_nThsMode = thsStopAndEngine;

//    double rTyreDiameter = 165 * 0.65 * 2 + 15 * 25.4;
    double rTyreDiameter = 285 * 2;
    double rTyreGirth = rTyreDiameter * Math.PI;
    double rRingGear = (int)((3.927 / rTyreDiameter / Math.PI / 60 * 1000 * 1000) * rSpeed);

    m_planetaryGear.setRingGearSpeed(rRingGear);
    m_planetaryGear.setPlaCarrierSpeed(rEngineRpm);

    //  発電機の回転数が極端にならないようにエンジンの回転数を調節
    if (m_planetaryGear.getSunGearSpeed() > 6000)
      m_planetaryGear.setSunGearSpeed(6000);
    else if (m_planetaryGear.getSunGearSpeed() < -4500)
      m_planetaryGear.setSunGearSpeed(-4500);
    if ((m_nThsMode == thsNormal) && (m_planetaryGear.getSunGearSpeed() < 300))
      m_nThsMode = thsNormalOD;   //  発電機は 300rpm で発電を失効する（と仮定する）このとき、オーバードライブ状態であるとみなす。
    if ((m_nThsMode == thsFullAccel) && (m_planetaryGear.getSunGearSpeed() < 300))
      m_nThsMode = thsFullAccelOD;   //  発電機は 300rpm で発電を失効する（と仮定する）このとき、オーバードライブ状態であるとみなす。

    m_planetaryGear.move(rMilliSec / 100);  //  プラネタリギアを回転させる

    switch (m_nThsMode) {
    case thsStart:
    case thsFullAccel:
    case thsFullAccelOD:
      m_rBatterySOC -= (rSpeed) * 0.0000022 * rMilliSec + (rTargetSpeed - rSpeed) * 0.0000015 * rMilliSec;
      break;
    case thsKaisei:
    case thsKaiseiAndEngine:
      m_rBatterySOC += (rSpeed - rTargetSpeed) * 0.000001 * rMilliSec;
      break;
    case thsNormal:
    case thsNormalOD:
      m_rBatterySOC += 0.000030 * rMilliSec;
      break;
    case thsStopAndEngine:
      m_rBatterySOC += 0.00010 * rMilliSec;
      break;
    case thsStop:
      break;
    }
  }

  /**
   * エンジンの回転数を取得する
   * @return エンジンの回転数(rpm)
   */
  public double getEngineRpm() {
    return m_planetaryGear.getPlaCarrierSpeed();
  }
  /**
   * 車速を取得する
   * @return 車速(km/h)
   */
  public double getSpeed() {
//    double rTyreDiameter = 165 * 0.65 * 2 + 15 * 25.4;
    double rTyreDiameter = 285 * 2;
    double rTyreGirth = rTyreDiameter * Math.PI;
    return (m_planetaryGear.getRingGearSpeed() / 3.927 * rTyreDiameter * Math.PI * 60 / 1000 / 1000);
  }

  /**
   * シミュレーションに使うプラネタリギアオブジェクト
   * @return プラネタリギアオブジェクト
   */
  public PlanetaryGear getPlanetaryGear() {
    return m_planetaryGear;
  }

  /**
   * THS(Toyota Hybrid System) の状態を取得
   * @return THS の状態
   */
  public int getThsMode() {
    return m_nThsMode;
  }

  /**
   * ガソリンを消費しているか判定
   * @return ガソリンを消費しているとき true
   */
  public boolean isGasoline() {
    return m_bGasoline;
  }
  /**
   * モーターを使用しているか判定
   * @return モーターで走っているかエンジンをアシストしているとき true
   */
  public boolean isMotor() {
    return m_bMotor;
  }
  /**
   * 回生ブレーキ動作中か判定
   * @return 回生ブレーキ動作中のとき true
   */
  public boolean isKaisei() {
    return m_bKaisei;
  }

  /**
   * プラネタリギアのリングギア（モーター直結・ファイナルギアへ接続）
   * @return プラネタリギアのリングギア
   */
  public Gear getRingGear() {
    return m_planetaryGear.getRingGear();
  }
  /**
   * プラネタリギアのプラネタリキャリア（エンジンに直結）
   * @return プラネタリギアのプラネタリキャリア
   */
  public PlanetaryCarrier getPlanetaryCarrier() {
    return m_planetaryGear.getPlanetaryCarrier();
  }
  /**
   * プラネタリギアのサンギア（発電機に直結）
   * @return プラネタリギアのサンギア
   */
  public Gear getSunGear() {
    return m_planetaryGear.getSunGear();
  }

  /**
   * リングギアの回転速度を取得する
   * @return リングギアの回転速度(rpm)
   */
  public double getRingGearSpeed() {
    return m_planetaryGear.getRingGearSpeed();
  }
  /**
   * プラネタリキャリアの回転速度を取得する
   * エンジン回転数と等価となる
   * @return プラネタリキャリアの回転速度(rpm)
   */
  public double getPlaCarrierSpeed() {
    return m_planetaryGear.getPlaCarrierSpeed();
  }
  /**
   * サンギアの回転速度を取得する
   * 発電機の回転数となる
   * @return サンギアの回転速度(rpm)
   */
  public double getSunGearSpeed() {
    return m_planetaryGear.getSunGearSpeed();
  }
  /**
   * バッテリー残量を取得
   * @return バッテリー残量(%)
   */
  public double getBatterySOC() {
    return m_rBatterySOC;
  }
  /**
   * バッテリー残量が減り、充電の必要があるときに true
   * エンジンが常に掛かった状態となる
   * @return 充電の必要があるときに true
   */
  public boolean isEngineRequest() {
    return m_bEngineRequest;
  }
  /**
   * 出力制限警告灯（いわゆるカメ）の表示状態を判定
   * @return 出力制限警告灯が点灯しているとき true
   */
  public boolean isPowerSaveMode() {
    return m_bPowerSaveMode;
  }

  /**
   * 直前の drive メソッドで使用された燃料使用量(L)
   * @param rMilliSec 経過秒数
   * @return 燃料使用量(L)
   */
  protected double getUseFuel(double rMilliSec) {
    double rUseFuel = 0;
    String sShiftPosition = getShiftPosition();
    if (isGasoline()) {
      double rBatteryRatio = 1.0 + (m_rBatterySOC - 60.0) / 20.0 * 0.10;  //  バッテリーＳＯＣを基準として多ければ燃費を良くし、少なければ燃費を悪くする
      if (sShiftPosition.equals("D") || sShiftPosition.equals("R"))
        rUseFuel = Math.pow(getEngineRpm() / 1000, 1.4) * 1.7 / 3600 / 1000 * rMilliSec / rBatteryRatio;
      else
        rUseFuel = Math.pow(getEngineRpm() / 1000, 1.5) * 1.54 / 3600 / 1000 * rMilliSec / rBatteryRatio;
    }
    return rUseFuel;
  }
}