import xxxsimulatorxxx.*;

Fahrzeug fahrzeug;
Integrator integrator;
FahrzeugModell2     modell;

float anim_phi=0.0f; //Lenkwinkel
float anim_beta=0.0f; //Chassis-Verdrehung gegenüber Inertialkoordinatensystem
float anim_x=0.0f;
float anim_y=0.0f;

double dt = 0.1;
double  t = 0.0;


double[] y;
double[] y0;
double[] y_alt;

boolean started = false;

//Neue Parameter, die den Parcours festlegen:
double RADIUSPARCOURS = 1.0;

//Neue Parameter für Regler global definieren, um sie in
//der Fehlerfunktion verfügbar zu haben:

double[] PID = {2.0,1.0,1.0};

void setup()
{  
    fahrzeug = new Fahrzeug(this);    
    modell = new FahrzeugModell2();
    
    y = new double[modell.getGleichungsanzahl()];
    y0 = new double[] {0.0,0.9,HALF_PI,0.0,0.0}; //x, y, beta
    y[0] = y0[0];
    y[1] = y0[1];
    y[2] = y0[2];
    y[3] = y0[3]; //Aufintegrieren von sensorwert (Regeldifferenz)
    y[4] = y0[4]; //Aufintegrieren der Servo-Lenkwinkelgeschw.
    y_alt = new double[modell.getGleichungsanzahl()];
    integrator = new Integrator(modell);

    frameRate((int)(1.0/dt));
    
     size(800,800);
   
    smooth();
  
    started = true;  
}

void draw()
{
    translate(width/2,height/2);
    scale(3.0f,3.0f);

    clear();
    background(200,200,255);
    
    anim_x = 100.0f*(float)y[0]; //in cm umrechnen    
    anim_y = 100.0f*(float)y[1];        
    anim_beta = (float)y[2]; //Chassis-Winkel
    anim_phi  = (float)modell.phi;  //Lenkwinkel

    //Parcours einzeichenen:
    noFill();
    stroke(255,0,0);
    ellipse(0,0,(float)(RADIUSPARCOURS*200.0),(float)(RADIUSPARCOURS*200.0));   
    
    fahrzeug.draw(anim_x,anim_y,anim_beta,anim_phi);
  
    translate(-width/2,-height/2);
    
    for(int i=0;i<y.length;i++)
        y_alt[i] = y[i];
  
    integrator.zeitschritt(y,y_alt,t,dt); //Ergebnis wird in y gelegt

    modell.sensorwert_alt = modell.sensorwert;
    
    t+=dt;   
}

void keyPressed()
{
    //Reagieren auf Pfeiltasten mit Seiten umblättern
    if(key==CODED)
    {
      if(keyCode==RIGHT)
      {
        modell.phi-=0.05;
      }
      if(keyCode==LEFT)
      {
        modell.phi+=0.05;
      }
    }
}


