#include <QtGui>
#include <QtOpenGL>
#include "sailboat.h"
#include "mainwindow.h"
#include <QGLContext>
#include <QDebug>
#include <QFile>
QFile info("info.txt");
QFile traj("traj.m");
QTextStream mottraj(&traj);


extern double dt;
int k1=0;


sailboat::sailboat()
{   x=0.0; y=40.0; theta =0.0; v = 1; omega = 0.0;  phi = 0.5; phiPoint = 0;
    Jx = 3000.0; Jz = 10000.0; // moments d'inertie
    beta=0.1;  rg=2.0;  alphatheta=6000;  m=300.0;
    alphaf=1.0;  rv=1.0;  alphag=2000.0; l=1.0;  alphav=1000.0;
    a=2;
    psi=-0.5*M_PI;  //wind angle
    thetabar=0;
    eta = 16000;     hv = 4.00;  //Roulis
    zeta=M_PI/4;
    mload=2000;
    L0=50;
    loadx=0; loady=1; loadvx=0; loadvy=0.001;
    zeta0=0.2;  //demi-angle du secteur
    eps=1;
    Radiusfield=3*L0;
    //Radiusfield=10*L0;

    trajLoadx.clear();
    trajLoady.clear();

    if (!info.open(QIODevice::WriteOnly | QIODevice::Text))
    {     qDebug()<<"fichier info ouvert par ailleurs"; return;   }

    if (!traj.open(QIODevice::WriteOnly | QIODevice::Text))
    {     qDebug()<<"fichier traj ouvert par ailleurs"; return;   }

    mottraj<<"hold on \n";
    mottraj<<"T=[ \n";


}

sailboat::~sailboat()
{
    mottraj<<"]; \n";
    mottraj<<"plot(T(:,1),T(:,2),'blue','LineWidth',1);\n";
    mottraj<<"plot(T(:,3),T(:,4),'black','LineWidth',2);\n";
    mottraj<<"R="<<Radiusfield<<"; t=0:0.01:2*pi; plot(R*cos(t),R*sin(t),'red'); \n";
    info.close();
    traj.close();

}

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




void sailboat::Controller()   //voir www.ensta-bretagne.fr/jaulin/polyrobots.pdf
{      alphastar=atan2(loady,loadx)+0.5*M_PI+atan(0.1*(hypot(loadx,loady)-Radiusfield));  //vector field
       alphastar=alphastar+sin((alphastar-atan2(loadvy,loadvx)));   // proportional control of the load

       double beta0=0.5*M_PI-zeta-zeta0;
        double alf=alphastar-psi;
        alphabar=psi+atan2(min(fabs(sin(alf)),sin(beta0))*sign(sin(alf)),max(fabs(cos(alf)),cos(beta0))*sign(cos(alf)));
        if (cos(alphabar-alpha)<cos(zeta0))     eps=sign(sin(alphabar-alpha));
        thetabar=alpha+eps;
        double dtheta=theta-thetabar;
        deltag=0.5*((cos(dtheta)>0)*sin(dtheta)
                      +(cos(dtheta)<0)*sign(sin(alpha-thetabar))); //empanage ou virement suivant le vent pour éviter les noeuds
        deltavmax=0.5*M_PI*(0.5*(cos(psi-thetabar)+1));
}




void sailboat::Clock()  // The model is described in "L. Jaulin Modélisation et commande d'un bateau à voile, CIFA2004, Douz (Tunisie)"
{   Controller();

    if (k1%5==0) {mottraj<<x<<", "<<y<<", "<<loadx<<", "<<loady<<"; \n";}

    k1++;
    if (k1%1000==0)   // on largue une bouee
    {  trajLoadx.push_back(loadx); trajLoady.push_back(loady);   }


            alpha=atan2(y-loady,x-loadx);     //angle du cable
            L=hypot(y-loady,x-loadx);
    double xw_ap=a*cos(psi-theta)-v;
    double yw_ap=a*sin(psi-theta);
    double psi_ap=atan2(yw_ap,xw_ap);   //Apparent wind
    double a_ap=sqrt(xw_ap*xw_ap+yw_ap*yw_ap);
    gamma_sail=cos(psi_ap)+cos(deltavmax);
    if (gamma_sail<0) {deltav=M_PI+psi_ap;} //voile en drapeau
    else  if (sin(-psi_ap)>0) deltav=deltavmax;   else deltav=-deltavmax;
    fg = alphag*v*sin(deltag);
    fv = alphav*a_ap*sin(deltav-psi_ap);
    double fc = exp((L-L0));  // force du cable

    x += (v*cos(theta)+beta*a*cos(psi)  - fc*cos(alpha) )*dt;
    y += (v*sin(theta)+beta*a*sin(psi)  - fc*sin(alpha) )*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-fc*cos(alpha-theta))*dt;
    phiPoint += (-phiPoint+fv*hv*cos(deltav)*cos(phi)/Jx - 10000*9.81*sin(phi)/Jx)*dt ;
    phi += phiPoint * dt;

    double loadv=hypot(loadvx,loadvy);

    loadvx += (1/mload)*(fc*cos(alpha)-10*loadv*loadvx)*dt;
    loadvy += (1/mload)*(fc*sin(alpha)-10*loadv*loadvy)*dt;
    loadx += loadvx*dt;
    loady += loadvy*dt;
}
