//---------------------------------------------------------------------------
#include <vcl.h>
#include "Unit1.h"
#include "Unit2.h"
#pragma hdrstop
//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma link "OpenGLPanel"
#pragma resource "*.dfm"
TForm1 *Form1;
double dAngle=0.1;
//---------------------------------------------------------------------------
#include <math.h>
#include <stdlib.h>
#include <stdio.h>



//---------------------------------------------------------------------------

void traceBoite(box P, float color1,float color2,float color3,bool fils_de_fer)
 {
                        glBegin(fils_de_fer ? GL_LINE_LOOP : GL_POLYGON );
                        glColor3f(color1,color2,color3);
                        glVertex3f(P[2].inf,P[1].inf,-P[3].inf);
                        glVertex3f(P[2].inf,P[1].sup,-P[3].inf);
                        glVertex3f(P[2].sup,P[1].sup,-P[3].inf);
                        glVertex3f(P[2].sup,P[1].inf,-P[3].inf);
                        glEnd();
                        glBegin(GL_POLYGON);
                        glVertex3f(P[2].inf,P[1].inf,-P[3].sup);
                        glVertex3f(P[2].inf,P[1].sup,-P[3].sup);
                        glVertex3f(P[2].sup,P[1].sup,-P[3].sup);
                        glVertex3f(P[2].sup,P[1].inf,-P[3].sup);
                        glEnd();
}



 //--------------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner)
    : TForm(Owner)
{       Fout.open("info.txt",fstream::out);
        GPSfinaly=264.10;
        GPSfinalx=689.96;
        errormine=interval(-1000,1000);
        errorDistSonar=interval(-1.0,1.0);


        CheckListBoxGraphic->Checked[0]=true;
        CheckListBoxGraphic->Checked[1]=false;
        CheckListBoxGraphic->Checked[2]=true;
        CheckListBoxGraphic->Checked[3]=true;
        CheckListBoxGraphic->Checked[4]=false;
        NoSolution=false;
        int i;

        dT=0.1; //R=15;
        cam.x = -6;   cam.y = 4;  cam.z = -1; // cam.psi = 53;  cam.v = 1, cam.grosseur=1  ;
        double lonOrg=-4.458227931;   //degrés décimaux
        double latOrg=48.212920614;
        mines = vector <Mine> (6);
        mines[0].latPrior=48+13.096/60.0; mines[0].lonPrior=-4-27.190/60.0;  mines[0].zPrior=19;
        mines[1].latPrior=48+13.099/60.0; mines[1].lonPrior=-4-27.101/60.0;  mines[1].zPrior=19;
        mines[2].latPrior=48+13.100/60.0; mines[2].lonPrior=-4-27.042/60.0;  mines[2].zPrior=19;
        mines[3].latPrior=48+12.826/60.0; mines[3].lonPrior=-4-27.496/60.0;  mines[3].zPrior=19;
        mines[4].latPrior=48+12.840/60.0; mines[4].lonPrior=-4-27.425/60.0;  mines[4].zPrior=20;
        mines[5].latPrior=48+12.844/60.0; mines[5].lonPrior=-4-27.367/60.0;  mines[5].zPrior=20;


        for (int k=0;k<mines.size();k++)
          {  mines[k].yPrior=(mines[k].lonPrior-lonOrg)*60*1852*cos(mines[k].latPrior*PI/180);
             mines[k].xPrior= (mines[k].latPrior-latOrg)*60*1852;
             mines[k].P=box(mines[k].xPrior+errormine,mines[k].yPrior+errormine,interval(-oo,oo));
          }

        TimerAffichage->Interval = 100;



        LireData();
        kmax=V.size();

        Ping p;
        p.t=7054.0; p.latmes=48.218327;	p.lonmes=-4.451778;p.latref=48.218317;	p.lonref=-4.451683; p.mine=1;p.distance=52.42+errorDistSonar; pings.push_back(p);
        p.t=7092.0; p.latmes=48.218373;	p.lonmes=-4.450850;p.latref=48.218333;	p.lonref=-4.450700; p.mine=2;p.distance=12.47+errorDistSonar; pings.push_back(p);
        p.t=7374.0; p.latmes=48.218337;	p.lonmes=-4.451733;p.latref=48.218317;	p.lonref=-4.451683; p.mine=1;p.distance=54.40+errorDistSonar; pings.push_back(p);
        p.t=7748.0; p.latmes=48.218293;	p.lonmes=-4.453302;p.latref=48.218267;	p.lonref=-4.453167; p.mine=0;p.distance=52.68+errorDistSonar; pings.push_back(p);
        p.t=9038.0; p.latmes=48.218358;	p.lonmes=-4.451792;p.latref=48.218317;	p.lonref=-4.451683; p.mine=1;p.distance=27.73+errorDistSonar; pings.push_back(p);
        p.t=9688.0; p.latmes=48.214167;	p.lonmes=-4.456183;p.latref=48.214067;	p.lonref=-4.456117; p.mine=5;p.distance=26.98+errorDistSonar; pings.push_back(p);
        p.t=10024.0;p.latmes=48.214065;	p.lonmes=-4.457045;p.latref=48.214000;	p.lonref=-4.457083; p.mine=4;p.distance=37.90+errorDistSonar; pings.push_back(p);
        p.t=10817.0;p.latmes=48.213865;	p.lonmes=-4.458235;p.latref=48.213767;	p.lonref=-4.458267; p.mine=3;p.distance=36.71+errorDistSonar; pings.push_back(p);
        p.t=11172.0;p.latmes=48.213855;	p.lonmes=-4.458285;p.latref=48.213767;	p.lonref=-4.458267; p.mine=3;p.distance=37.37+errorDistSonar; pings.push_back(p);
        p.t=11232.0;p.latmes=48.214072;	p.lonmes=-4.457117;p.latref=48.214000;	p.lonref=-4.457083; p.mine=4;p.distance=31.03+errorDistSonar; pings.push_back(p);
        p.t=11279.0;p.latmes=48.214182;	p.lonmes=-4.456185;p.latref=48.214067;	p.lonref=-4.456117; p.mine=5;p.distance=33.51+errorDistSonar; pings.push_back(p);
        p.t=11688.0;p.latmes=48.218357;	p.lonmes=-4.451752;p.latref=48.218317;	p.lonref=-4.451683; p.mine=1;p.distance=15.05+errorDistSonar; pings.push_back(p);

        int k1=0; //numéro du ping
        pingNb = vector<int> (kmax);
        for (int k=0; k<kmax;k++)
        { pingNb[k]=-1;
          if (pings[k1].t==temps[k])
           { pingNb[k]=k1;
             pings[k1].k=k;
             Fout <<"k="<<k<<", k1="<<k1<<", mine = "<<pings[k1].mine
                         <<", xmine=" <<mines[pings[k1].mine].xPrior
                         <<", ymine=" <<mines[pings[k1].mine].yPrior
                         <<", zmine=" <<mines[pings[k1].mine].zPrior
                         <<"\n";

             k1++;
           };
        }

        SimuEuler();

        for (int k1=0; k1<pings.size();k1++)
        {    int k=pings[k1].k;
             double xm=mines[pings[k1].mine].xPrior;
             double ym=mines[pings[k1].mine].yPrior;
             double zm=mines[pings[k1].mine].zPrior;
             interval xe=P_Euler[k][1];
             interval ye=P_Euler[k][2];
             interval ze=P_Euler[k][3];
             interval re=Sqrt(Sqr(xe-xm)+Sqr(ye-ym)+Sqr(ze-zm));
             interval rm=pings[k1].distance;
             Form1->Fout <<"\n k="<<k<<", k1="<<k1<<", mine = "<<pings[k1].mine
                         <<", xm=" <<xm
                         <<", ym=" <<ym
                         <<", zm=" <<zm
                         <<", Px="<<xe<<", Py="<<ye<<", Pz="<<ze<<", re="<<re
                         <<", rm="<<rm
                         <<", dr="<<Abs(re-rm);
        }


        Contract_Init();
        Contract_Forward();
//        Contract_Mine();
//        Contract_Backward();
}

//----------------------------------------------------------------------------
void __fastcall TForm1::OpenGLPanel1Init(TObject *Sender)
{
        glViewport(0,0,OpenGLPanel1->Width,OpenGLPanel1->Height);
	glMatrixMode(GL_PROJECTION);
        if ( OpenGLPanel1->Height==0)
     	        gluPerspective(70, (GLdouble)OpenGLPanel1->Width, 1.0, 2000.0);
        else
                gluPerspective(70, (GLdouble)OpenGLPanel1->Width/
                           (GLdouble)OpenGLPanel1->Height,1.0, 2000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

        glEnable(GL_DEPTH_TEST);
        glClearColor(0.6,0.6,1.0,0.1); //ciel
        GLfloat LightAmbient[]={ 1.0,1.0,1.0,1.0};
        GLfloat LightDiffuse[]={ 1.0f, 1.0f, 1.0f, 1.0f };
        GLfloat light_specular[]={1.0,1.0,1.0,1.0};
        GLfloat LightPosition[]= {10.0f,10.0f,10.0f, 1.0f };
        glEnable(GL_LIGHTING);
        glLightfv(GL_LIGHT0, GL_AMBIENT, LightAmbient);
        glLightfv(GL_LIGHT0, GL_DIFFUSE, LightDiffuse);
        glLightfv(GL_LIGHT0,GL_SPECULAR,light_specular);
        glLightfv(GL_LIGHT0, GL_POSITION,LightPosition);
	glEnable(GL_LIGHT0);
}




//---------------------------------------------------------------------------
void __fastcall TForm1::OpenGLPanel1Paint(TObject *Sender)
{
        // coordonnées du point de référence de l'auv
        // coordonnées du point du véhicule qui est centré dans la scène
        // angles d'attitude du véhicule

        double roll,pitch,head;

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      	glLoadIdentity();

        cam.v= TrackBarCamV->Position ;
        cam.z= 0.1*TrackBarCamZ->Position ;
        cam.x+=cam.v*sin(cam.psi*DegToRad)/100;
        cam.y+=cam.v*cos(cam.psi*DegToRad)/100;
        cam.psi=TrackBarCamPsi->Position ;
        cam.grosseur=TrackBarGrosseur->Position ;
        double Ciblex=cam.x+100*sin(cam.psi*DegToRad);
        double Cibley=cam.y+100*cos(cam.psi*DegToRad);
        double Ciblez=TrackBarCamCiblez->Position ;
        gluLookAt(cam.x,cam.y,cam.z,Ciblex,Cibley,Ciblez,0,0,1);

        /* Brouillard */
        if (TrackBarFog->Position==0) glDisable(GL_FOG);
        else
        {       glEnable(GL_FOG);
                float fog_color[4] = {0, 0.32, 0.5, 1};
                glFogi(GL_FOG_MODE,GL_EXP);
                glFogf(GL_FOG_START,1);
                glFogf(GL_FOG_END,15);
                glFogfv(GL_FOG_COLOR,fog_color);
                glFogf(GL_FOG_DENSITY,TrackBarFog->Position/1000.0);
         }

        GLfloat amb[4] = { 1, 1, 1, 0.5 };
        GLfloat dif[4] = { 1.0, 1.0, 1.0, 0.5 };
        GLfloat spec[4] = { 2, 2, 2, 1 };
        glEnable(GL_COLOR_MATERIAL);      // reflet
        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, dif);
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, amb);
        glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, spec);
        glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 25.0);
        glColor3f(0.6,0,0);   // Sphere rouge
        GLUquadricObj* q=gluNewQuadric();
        gluQuadricDrawStyle(q,GLU_FILL);
        gluSphere(q,0.5,10,10);
        gluDeleteQuadric(q);

        CheckListBoxGraphicClick(Sender);

        if (WithDrawMine)
        for (int km=0;km<mines.size();km++)
        {  glColor3f(0,0,0);
           glPushMatrix();
           glTranslatef(mines[km].yPrior,mines[km].xPrior,-mines[km].zPrior);
           GLUquadricObj* q1=gluNewQuadric();
           gluQuadricDrawStyle(q1,GLU_FILL);
           gluSphere(q1,0.2*cam.grosseur,15,15);
           gluDeleteQuadric(q1);
           glPopMatrix();
           traceBoite(mines[km].P, 0.2,0.2,0.2,true);
        }

           //GPS final
           glColor3f(0.4,0.9,0.4);
           glPushMatrix();
           glTranslatef(GPSfinaly,GPSfinalx,0);
           GLUquadricObj* q1=gluNewQuadric();
           gluQuadricDrawStyle(q1,GLU_FILL);
           gluSphere(q1,0.4*cam.grosseur,15,15);
           gluDeleteQuadric(q1);
           glPopMatrix();


        glColor3f(1,0,0);    //x,y l'AUV
        glPushMatrix();
           glTranslatef(0,0,0.21);
           GLUquadricObj* q6=gluNewQuadric();
           gluQuadricDrawStyle(q6,GLU_FILL);
           gluSphere(q6,0.1*cam.grosseur,5,5);
           gluDeleteQuadric(q6);
        glPopMatrix();


        if (WithDrawMer)
        {       glBegin(GL_POLYGON); //mer
                glColor3f(0.0,0.0,0.6);
                glVertex3f(-100000,-100000,-0.1);      glVertex3f(100000,-100000,-0.1);
                glVertex3f(100000,100000,-0.1);        glVertex3f(-100000,100000,-0.1);
                glEnd();
        }

        {
        glBegin(GL_POLYGON); //fond
                if (NoSolution) glColor3f(1,0,0);
                else glColor3f(0.96,0.96,0.95);
                glVertex3f(-100000,-100000,-30);      glVertex3f(100000,-100000,-30);
                glVertex3f(100000,100000,-30);        glVertex3f(-100000,100000,-30);
        glEnd();
        }

        glEnable(GL_NORMALIZE);
        if  (WithDrawAUV)
        {       dAngle=dAngle+0.03;
                glPushMatrix();
                AUV1.x=0;  AUV1.y=0; AUV1.z=2; AUV1.phi=0; AUV1.theta=0; AUV1.psi=0;
                AUV1.Draw();
                AUV1.x=6;                      AUV1.phi=0; AUV1.theta=0; AUV1.psi=1+dAngle;
                AUV1.Draw();
                AUV1.x=12;                                 AUV1.theta=1+dAngle; AUV1.psi=0;
                AUV1.Draw();
                AUV1.x=18;                     AUV1.phi=1+dAngle; AUV1.theta=0; AUV1.psi=0;
                AUV1.Draw();
                glPopMatrix();
        }

        if (cam.grosseur>0)
        {
                for (int k=0;k<kmax;k=k+50)            //EULER
                {       glColor3f((1.0*k/kmax),0,(1-1.0*k/kmax));
                        glPushMatrix();
                        glTranslatef(P_Euler[k][2].inf,P_Euler[k][1].inf,-P_Euler[k][3].inf);
                        GLUquadricObj* q1=gluNewQuadric();
                        gluQuadricDrawStyle(q1,GLU_FILL);
                        gluSphere(q1,0.1*cam.grosseur,3,3);
                        gluDeleteQuadric(q1);
                        glPopMatrix();
                }
        }
        if (WithDrawPing)
        for (int k=0;k<kmax;k++)
        {  if (pingNb[k]!=-1)
                {       glColor3f(0.5,0.5,0.5);
                        glPushMatrix();
                        glTranslatef(P_Euler[k][2].inf,P_Euler[k][1].inf,-P_Euler[k][3].inf);
                        GLUquadricObj* q1=gluNewQuadric();
                        gluQuadricDrawStyle(q1,GLU_FILL);
                        gluSphere(q1,0.13*cam.grosseur,5,5);
                        gluDeleteQuadric(q1);
                        glPopMatrix();
                }
        }

        if (WithDrawEnveloppe)
        for (int k=0;k<kmax;k=k+50)  traceBoite(P[k], 0.9,0.6,0.5,true);


        if (WithDrawEnveloppe)
        for (int k=0;k<kmax;k++)
          if (pingNb[k]!=-1)   traceBoite(P[k], 0.8,0.8,0.8,true);

}

//---------------------------------------------------------------------------
void __fastcall TForm1::TimerAffichageClock(TObject *Sender)
{
        char text[128];
        sprintf(text,"%.2lf",cam.psi);
        EditPsiCam->Text = text;
        sprintf(text,"%.2lf",cam.y);
        EditXcam->Text = cam.x;
        sprintf(text,"%.2lf",cam.x);
        EditYcam->Text = cam.y;
        sprintf(text,"%.2lf",-cam.z);
        EditZcam->Text = cam.z;
        OpenGLPanel1->Repaint();
}

//---------------------------------------------------------------------------

void __fastcall TForm1::CheckListBoxGraphicClick(TObject *Sender)
{
            WithDrawAUV=CheckListBoxGraphic->Checked[0];
            WithDrawMer=CheckListBoxGraphic->Checked[1];
            WithDrawPing=CheckListBoxGraphic->Checked[2];
            WithDrawMine=CheckListBoxGraphic->Checked[3];
            WithDrawEnveloppe=CheckListBoxGraphic->Checked[4];
}
//---------------------------------------------------------------------------

void __fastcall TForm1::ContractForwardClick(TObject *Sender)
{
        Contract_Forward()==0;


}
//---------------------------------------------------------------------------
void __fastcall TForm1::ButtonContractInitClick(TObject *Sender)
{
     Contract_Init();
}
//---------------------------------------------------------------------------
void __fastcall TForm1::ContractMineClick(TObject *Sender)
{       Contract_Mine();
}
//---------------------------------------------------------------------------
void __fastcall TForm1::ButtonContractBackwardClick(TObject *Sender)
{
       Contract_Backward();
}
//---------------------------------------------------------------------------
void __fastcall TForm1::ButtonContractEssaiClick(TObject *Sender)
{
       Contract_Essai();
}
//---------------------------------------------------------------------------

void __fastcall TForm1::FormClose(TObject *Sender, TCloseAction &Action)
{      Fout<<"Je suis dans FormClose \n";
       for (int k=0;k<mines.size();k++)
          {  Fout<<"k="<<k<<"  mine= "<<mines[k].P<<"\n";
             Fout<<"xPrior="<<mines[k].xPrior<<", yPrior="<<mines[k].yPrior<<", zPrior="<<mines[k].zPrior<<"\n";
          }
       Fout <<"P[kmax-1] = "<<P[kmax-1]<<"\n";
        Fout.close();

}
//---------------------------------------------------------------------------

