#include "controller.h"
#include <QtCore>
#include <QDebug>
extern double t, dt;


controller::controller()
{   dell=0; deltag=0;
    q=1; zeta=1.0;
}

double sign(double a, double eps=0)  // eps= accuracy
 { if (a<=-eps) return -1.0;
   else if (a>=eps) return 1.0;
   else return a/eps;
 }

/*double controller::fieldMill(double x,double y,double psi)
{
    thetaProj=M_PI+psi-sign(sin(M_PI+psi-atan2(-y,-x)))*zeta;
    return (thetaProj);
};*/



void controller::Clock(double x,double y,double theta,double ell, double psi)
{       if (q==1)    { if (cos(psi-theta)>0.95) q=2;}
        if (q==2)    { if (cos(psi-theta)<-0.95)
                            if (cos(psi-atan2(y,x))>-cos(zeta))  //pas dans le cone
                                 {if (sin(psi-atan2(y,x))>0) q=3; else q=4 ;
                                  t0=t;}
                            else q=1;

                     }
        if ((q==3)&&(t-t0>30)) q=1;
        if ((q==4)&&(t-t0>30)) q=1;

        if (q==1) thetabar=psi;            // On abat
        if (q==2) thetabar=M_PI+psi+zeta;  // On lofe
        if (q==3) thetabar=M_PI+psi-zeta;
        if (q==4) thetabar=M_PI+psi+zeta;




            deltag=0.5*sin(theta-thetabar);
            if ((cos(theta-thetabar)<0) ||(q==1)||(q==2))
                 deltag=0.5*sign(deltag);
            double ellDesire=1.1*pow((0.5*(cos(psi-theta)+1)),1.46); // 0.874=(log(1.1)-log(0.4))/log(2);
            if (ellDesire>ell) dell=1; else dell=0; // unblock
}
