/**
Fahrzeugmodell:<br/>
Systemzusände:       <br/>
phi       <br/>
xA        <br/>
yA        <br/>

Ableitungen:       <br/>
wA       <br/>
vxA        <br/>
vyA        <br/>

*/
public class FahrzeugModell2 implements Modell
{
    //Modellparameter in Metern MUSS MIT PARAMETERN IN "Fahrzeug" übereinstimmen!!!
    double achsabstand = 0.08;
    double raddurchmesser = 0.058;
    double raddicke = 0.02;
    double laenge = 0.175;
  
  
    double phimin = 0.001; //Lenkwinkel, ab dem von Geradeausfahrt ausgegangen wird (in rad)!
  
    double xA;
    double yA;
    double beta; //Chassis-Winkel im Inertialkoordinatensystem
    
    double vxA; //Winkelgeschwindigkeit Punkt A
    double vyA; //Winkelgeschwindigkeit Punkt A
    double wA; //Winkelgeschwindigkeit Punkt A

    double vA; //Geschwindigkeit Punkt A in Fahrtrichtung in I'


    double xQ; //X-Koordinate Momentanpol in I'
    double yQ; //Y-Koordinate Momentanpol in I'
  
    //Vorgegeben wird:
    public double phi  = 0.0; //aktueller Lenkwinkel
    public double vB   = 0.1; //Geschwindigkeit Punkt B in Fahrtrichtung (Antriebsrad)

//Neu: Hilfsvariablen für PID-Regler:
    public double i_sensorwert = 0.0;
    public double sensorwert = 0.0;
    public double d_sensorwert = 0.0;
    public double sensorwert_alt = 0.0;
    public double phi_soll = 0.0;
    //3 Bilder für 45 Grad und 25 Bilder pro Sekunde
    //=> 45/(3/25) => 375 Grad/sek ==  
//    public double phiradprosekunde = 6.5;
    public double phiradprosekunde = 1.0;
  
    public int getGleichungsanzahl()
    {
        return 5;      
    }
  
    void steigung(double[] f, double[] yy,double t)
    {
        xA   = yy[0];
        yA   = yy[1];
        beta = yy[2];        

        phi = yy[4];

//Neu: Berechnung des Sensorwertes (Distanz A zu Parcourskreisrand):
        double xB = xA - Math.sin(beta)*laenge;
        double yB = yA + Math.cos(beta)*laenge;

        //a) Einheitsvektor nach A:
        double dd = Math.sqrt(xB*xB+yB*yB);
        sensorwert = RADIUSPARCOURS-dd;             
println(sensorwert);
//Neu: Regler
        double pid = PID[0]*sensorwert + PID[1]*i_sensorwert + PID[2]*(sensorwert-sensorwert_alt)/dt;
//        double pid = PI[0]*sensorwert + PI[1]*i_sensorwert;
//        double pid = PID[0]*sensorwert;

        i_sensorwert=y[3];
//        sensorwert_alt = sensorwert;

        phi_soll = -pid;
        
        if(phi_soll>HALF_PI)
            phi_soll = (double)HALF_PI;
        if(phi_soll<-HALF_PI)
            phi_soll = -(double)HALF_PI;

//phi = phi_soll;
        
        if(phi<phi_soll)
            f[4] = phiradprosekunde;
        else if(phi>phi_soll)            
            f[4] = -phiradprosekunde;
        else
            f[4] = 0.0;             
             
        if(phi<=phimin && phi>=-phimin)
        {
            //Modell für Geradeausfahrt
            wA = 0.0;
            vA = vB; //Radgeschw. und Geschw. in Punkt A müssen gleich sein, wenn der Lenkwinkel Null ist!
            vxA =  -Math.sin(beta)*vA;
            vyA =   Math.cos(beta)*vA;            
        }
        else
        {
            //Modell für Kurvenfahrt
            
            //1) Momentanpol in I':
            xQ = -laenge/Math.tan(phi);
            yQ = 0.0;
         
            //2) Winkelgeschwindigkeit um den Momentanpol bestimmen == Winkelgeschw. um A == wA:
            //2a) Distanz von Q zu Vorderrad B:
            double BQ = Math.sqrt(xQ*xQ + laenge*laenge);
            wA = vB/BQ;
            //Vorzeichen korrigieren:
            if(phi<0.0)
               wA=-wA;
            
            vA = -xQ*wA; //Geschw. Punkt A in I'  

            //...umrechnen ins Inertialkoordinatensystem
            vxA =  -Math.sin(beta)*vA;
            vyA =   Math.cos(beta)*vA;            
        }
      
 
        f[0] = vxA;
        f[1] = vyA; 
        f[2] = wA;
        f[3] = sensorwert; //Aufintegrieren der Regeldifferenz 
    }  
}

