#include <QDebug>
#include "robot.h"

extern double dt;

robot::robot()
{
    x=-20*20;y=10*20;theta=3.0;v=5;deltav=0;ell=0;
    omega = 0.0;
    Jz = 10000.0; // moments d'inertie
    beta=0.05;  rg=2.0;
    alphatheta=6000;
    m=300.0; alphaf=0.2;
    rv=1;  //dist mat_centre
    alphag=2000.0;    l=1.0;    E=0;    alphav=1000.0;
    a=4; psi=1.0*M_PI;
}

double sign(double a) { if (a<=0) return -1; else if (a>=0) return 1; }

void robot::Clock(double deltag,double dell)
{   ell=ell+dt*dell;   //blocker
    double xw_ap=a*cos(psi-theta)-v;
    double yw_ap=a*sin(psi-theta);
    psi_ap=atan2(yw_ap,xw_ap);
    a_ap=sqrt(xw_ap*xw_ap+yw_ap*yw_ap);
    gamma=cos(psi_ap)+cos(ell);
    flag_sail=(gamma<0);
    if (gamma<0) //voile en drapeau
       {  deltav=atan(tan(psi_ap));
          ell=fabs(deltav);
       }
    else  deltav=sign(sin(-psi_ap))*ell;
    fg = alphag*v*sin(deltag);
    fv = alphav*a_ap*sin(deltav-psi_ap);
    x += (v*cos(theta)+beta*a*cos(psi))*dt;
    y += (v*sin(theta)+beta*a*sin(psi))*dt;
    theta += omega*dt;
    omega += (1/Jz)*((l-rv*cos(deltav))*fv-rg*cos(deltag)*fg-alphatheta*omega*v)*dt;
    v     += (1/m)*(sin(deltav)*fv-sin(deltag)*fg-alphaf*v*v)*dt;
    E     += dt*fabs(fv)*l*dell;
}

