package revpend1;

import java.awt.*;

/**
 * <p>Title: </p>
 * <p>Description: </p>
 * <p>Copyright: Copyright (c) 2005</p>
 * <p>Company: </p>
 * @author not attributable
 * @version 1.0
 */
//倒立振り子シミュレーション
//参考文献　

//振り子の状態

public class StateR extends Frame {

  double Ang , Xp ;//角度、位置
  double dAng, ddAng;//角度の1,2次微分
  double dXp, ddXp;

  double ca, cc;//運動方程式係数
  double cb , cd ;

  double ck1, ck2, ck3, ck4;//外部力係数
  double step ;
  double Time;
  int stepms,limitms;

  TextField textFieldAngle = new TextField();
  Label label1 = new Label();
  Label label2 = new Label();
  TextField textFieldPos = new TextField();
  TextField textFieldStep = new TextField();
  Label label3 = new Label();
  TextField textFieldLimit = new TextField();
  Label label4 = new Label();
  Label label5 = new Label();
  TextField textFieldPendW = new TextField();
  TextField textFieldPendL = new TextField();
  Label label6 = new Label();
  TextField textFieldBaseW = new TextField();
  TextField textFieldIM = new TextField();
  Label label7 = new Label();
  Label llabel8 = new Label();
  TextField textFieldck1 = new TextField();
  TextField textFieldck2 = new TextField();
  TextField textFieldck3 = new TextField();
  TextField textFieldck4 = new TextField();
  Label label8 = new Label();
  Label label9 = new Label();
  Label label10 = new Label();
  Label label11 = new Label();

  public StateR() {
//コンストラクタ：初期設定
    setSize(400,250);
    this.setLocation(100,200);

    //Setup();
    ca = 22.2; //状態方程式係数
    cc = -2.37;
    cb = -5.0;
    cd = 1.57;

    ck1 = 17.7;
    ck2 = 3.4; //理論値
  //ck2=2.8;
    ck3 = 1.1;
    ck4 = 1.32; //理論値
  //ck4=0.8;

    Ang = 0.1;
    Xp = 0.1;
    dAng = 0.0;
    dXp = 0.0;

    ca = 22.2;//状態方程式係数
    cc = -2.37;
    cb = -5.0;
    cd = 1.57;


    ck1 = 17.7;
    ck2 = 3.4;//理論値
    //ck2=2.8;
    ck3 = 1.1;
    ck4 = 1.32;//理論値
    //ck4=0.8;

    step=0.1;
    stepms=40;
    Time=0.0;

    //System.out.println("ca:"+ca+" cb:"+cb+" cc:"+cc+" cd:"+cd+"\n");

    try {
      jbInit();
    }
    catch(Exception e) {
      e.printStackTrace();
    }
  }

  public void initCond() {

    Setup();
    setupFB();//フィードバック係数

    double AngDeg = Double.parseDouble(textFieldAngle.getText());
    Ang=Math.PI*AngDeg/180.0;
    Xp = Double.parseDouble(textFieldPos.getText());
    stepms=Integer.parseInt(textFieldStep.getText());
    if(stepms<=20) {
      stepms = 20;
      textFieldStep.setText(Integer.toString(stepms));
    }
    //limitms=Integer.parseInt(textFieldLimit.getText());
    textFieldLimit.setText(Integer.toString(stepms*400));
    step=stepms/1000.0;
  }

  public void Next() {
    //次の時刻の値を求める
    double vu;
    vu = ck1 * Ang + ck2 * dAng + ck3 * Xp + ck4 * dXp;

    ddAng = ca * Ang + cc * vu;
    dAng += ddAng * step;
    Ang += dAng * step;

    ddXp = cb * Ang + cd * vu;
    dXp += ddXp * step;
    Xp += dXp * step;

    Time += step;
  }

  public void Setup(){
    //システムの物理パラメータから、状態方程式の係数を求める
    //System.out.println("begin setup \n");

    double pm,pM,pL;
    pm=0.36;pL=0.6;pM=0.6;
    pm=1.0;pM=1.0;pL=1.0;

    String st=textFieldPendW.getText();
    System.out.println("Setup:"+st+"\n");
    pm=Double.parseDouble(st);
    pL=Double.parseDouble(textFieldPendL.getText());
    pM=Double.parseDouble(textFieldBaseW.getText());

    double pg,pI;
    pg=9.8;
    pI=pm*pL*pL/3.0;//一様な太さの場合の理論値
    double pIR=Double.parseDouble(textFieldIM.getText());
    if(pIR >0.0) pI = pIR;
    else textFieldIM.setText(Double.toString(pI));

    //pI=0.014;
    double cx=pI*(pm+pM)+pm*pM*pL*pL;
    ca=pm*pL*(pm+pM)*pg/cx;
    cc=-pm*pL/cx;
    cb=-pm*pm*pL*pL*pg/cx;
    cd=(pI+pm*pL*pL)/cx;

    System.out.println("ca:"+ca+" cb:"+cb+" cc:"+cc+" cd:"+cd+" PI:"+pI+"\n");


  }

public void setupFB(){

  Complex c1=new Complex(-1,2);
  Complex c2=new Complex(-1,-2);
  Complex c3=new Complex(-2,1);
  Complex c4=new Complex(-2,-1);

  Complex p1=new Complex();
  Complex p2=new Complex();
  Complex p3=new Complex();
  Complex p4=new Complex();

  p1 = p1.prod1(c1,c2,c3,c4);
  p2 = p1.prod2(c1,c2,c3,c4);
  p3 = p1.prod3(c1,c2,c3,c4);
  p4 = p1.prod4(c1,c2,c3,c4);

  //p1.out();
  //p2.out();
  //p3.out();
  //p4.out();

  double dv=ca*cd -cb*cc;
  ck3 = p4.re/dv;
  ck4 = -p3.re/dv;
  ck1 = (-p2.re - ca -cd*ck3)/cc;
  ck2 = (p1.re - cd*ck4)/cc;

  double ckv;
  ckv = Double.parseDouble(textFieldck1.getText());
  if (ckv > 0) ck1 = ckv;
  else textFieldck1.setText(Double.toString(ck1));

  ckv = Double.parseDouble(textFieldck2.getText());
  if (ckv > 0) ck2 = ckv;
  else textFieldck2.setText(Double.toString(ck2));

  ckv = Double.parseDouble(textFieldck3.getText());
  if (ckv > 0) ck3 = ckv;
  else textFieldck3.setText(Double.toString(ck3));

  ckv = Double.parseDouble(textFieldck4.getText());
  if (ckv > 0) ck4 = ckv;
  else textFieldck4.setText(Double.toString(ck4));

  System.out.println(" ck1:"+ck1+" ck2:"+ck2+" ck3:"+ck3+" ck4:"+ck4);
}

  public void Trace(){
    //値をチェックする
    System.out.println("Time="+Time);
    System.out.println("ddAng="+ddAng +" dAng="+dAng+" Ang="+Ang);
    System.out.println("ddXp="+ddXp +" dXp="+dXp+" Xp="+Xp);
  }


  public void SystemConst(){

  }

  private void jbInit() throws Exception {
    textFieldAngle.setText("10");
    textFieldAngle.setBounds(new Rectangle(173, 54, 54, 23));
    this.setLayout(null);
    label1.setFont(new java.awt.Font("Dialog", 0, 10));
    label1.setText("初期角");
    label1.setBounds(new Rectangle(158, 33, 45, 21));
    label2.setFont(new java.awt.Font("Dialog", 0, 10));
    label2.setText("初期位置");
    label2.setBounds(new Rectangle(163, 88, 51, 16));
    textFieldPos.setText("0.0");
    textFieldPos.setBounds(new Rectangle(181, 101, 53, 22));
    this.setVisible(true);
    textFieldStep.setSelectionStart(10);
    textFieldStep.setText("40");
    textFieldStep.setBounds(new Rectangle(181, 150, 52, 20));
    label3.setFont(new java.awt.Font("Dialog", 0, 10));
    label3.setText("単位時間(ms)");
    label3.setBounds(new Rectangle(157, 135, 67, 15));
    textFieldLimit.setEditable(false);
    textFieldLimit.setText("2000");
    textFieldLimit.setBounds(new Rectangle(186, 200, 48, 21));
    label4.setFont(new java.awt.Font("Dialog", 0, 10));
    label4.setText("打ち切り時間");
    label4.setBounds(new Rectangle(156, 182, 68, 10));
    label5.setFont(new java.awt.Font("Dialog", 0, 10));
    label5.setText("棒の重さ(Kg)");
    label5.setBounds(new Rectangle(23, 33, 67, 19));
    textFieldPendW.setLocale(java.util.Locale.getDefault());
    textFieldPendW.setText("0.3");
    textFieldPendW.setBounds(new Rectangle(41, 55, 49, 21));
    textFieldPendL.setText("0.3");
    textFieldPendL.setBounds(new Rectangle(38, 97, 54, 20));
    label6.setFont(new java.awt.Font("Dialog", 0, 10));
    label6.setText("棒の長さ(m)");
    label6.setBounds(new Rectangle(20, 81, 60, 20));
    textFieldBaseW.setText("0.6");
    textFieldBaseW.setBounds(new Rectangle(46, 204, 57, 23));
    textFieldIM.setText("-1.0");
    textFieldIM.setBounds(new Rectangle(39, 145, 57, 19));
    label7.setFont(new java.awt.Font("Dialog", 0, 10));
    label7.setLocale(java.util.Locale.getDefault());
    label7.setText("台車の重さ(Kg)");
    label7.setBounds(new Rectangle(13, 177, 77, 21));
    llabel8.setFont(new java.awt.Font("Dialog", 0, 10));
    llabel8.setText("棒の回転モーメント");
    llabel8.setBounds(new Rectangle(17, 123, 100, 22));
    textFieldck1.setText("-1.0");
    textFieldck1.setBounds(new Rectangle(311, 55, 46, 23));
    textFieldck2.setText("-1.0");
    textFieldck2.setBounds(new Rectangle(306, 102, 57, 23));
    textFieldck3.setText("-1.0");
    textFieldck3.setBounds(new Rectangle(302, 151, 62, 21));
    textFieldck4.setText("-1.0");
    textFieldck4.setBounds(new Rectangle(303, 198, 62, 22));
    label8.setText("ck1");
    label8.setBounds(new Rectangle(280, 36, 46, 16));
    label9.setText("ck2");
    label9.setBounds(new Rectangle(283, 83, 36, 17));
    label10.setText("ck3");
    label10.setBounds(new Rectangle(285, 134, 39, 15));
    label11.setText("ck4");
    label11.setBounds(new Rectangle(286, 180, 35, 16));
    this.add(textFieldLimit, null);
    this.add(llabel8, null);
    this.add(label5, null);
    this.add(label6, null);
    this.add(textFieldPendL, null);
    this.add(textFieldPendW, null);
    this.add(textFieldIM, null);
    this.add(label7, null);
    this.add(textFieldBaseW, null);
    this.add(label1, null);
    this.add(textFieldAngle, null);
    this.add(label2, null);
    this.add(textFieldPos, null);
    this.add(label3, null);
    this.add(textFieldStep, null);
    this.add(label4, null);
    this.add(textFieldck1, null);
    this.add(textFieldck2, null);
    this.add(textFieldck3, null);
    this.add(textFieldck4, null);
    this.add(label8, null);
    this.add(label9, null);
    this.add(label10, null);
    this.add(label11, null);
  }
}