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

#include <vcl.h>
#pragma hdrstop
#include "box.h"
#include "Unit1.h"
#include "Unit2.h"
#include <vector>

#include "rmatrix.h"
#include <fstream>
#include "matrix_lib/tnt.h"
using namespace TNT;
#include "matrix_lib/jama_lu.h"
using namespace JAMA;

extern TForm1 *Form1;

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

#pragma package(smart_init)


int LireData()
{       FILE *f;
        double t1,phi1,theta1,psi1,vx1,vy1,vz1,depth1,altitude1;

        f = fopen("input.txt","r");    // vx,vy,vz,phi,theta,psi et immersion
        //state = fopen("state.txt","wt");  // x,y on ne s'en sert pas
        char line[256];
        if (f == NULL) return(0);
        while (fgets(line,256,f) != NULL)
        {   if (sscanf(line,"%lf%lf%lf%lf%lf%lf%lf%lf%lf",&t1,&phi1,&theta1,&psi1,&vx1,&vy1,&vz1,&depth1,&altitude1) != 9) return(0);
            Form1->temps.push_back(t1);
            (Form1->V).push_back(box(vx1+0.004*(vx1*interval(-1,1)+interval(-1,1)),
                                     vy1+0.004*(vy1*interval(-1,1)+interval(-1,1)),
                                     vz1+0.004*(vz1*interval(-1,1)+interval(-1,1))));
            Form1->phi.push_back(phi1+(0.01 * M_PI / 180.0)*interval(-1,1));
            Form1->theta.push_back(theta1+(0.01 * M_PI / 180.0)*interval(-1,1));
            Form1->psi.push_back(psi1+((0.2 * M_PI / 180.0) / cos(48.5 * M_PI / 180.0))*interval(-1,1));
            Form1->depth.push_back(interval(-0.5,0.5)+depth1*(interval(0.98,1.02)));
            Form1->altitude.push_back(altitude1+interval(-0.5,0.5));
        }
        fclose(f);
        return(1);
}


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



void SimuEuler(void)
/* Calcule les coordonnées du véhicule dans le repère TGL */
/* TGL: Trièdre Géographique Local */
/*      Origine: véhicule */
/*      Axe x: axe pointant vers le nord */
/*      Axe y: axe pointant vers l'est */
/*      Axe z: axe pointant vers le centre de la terre */

{       Form1->P_Euler=vector<box>(Form1->kmax);
        rmatrix p=Zeros(3,1);
        for (int k=0;k<Form1->kmax;k++)
            {   p.SetVal(3,1,Center(Form1->depth[k]));
                Form1->P_Euler[k]=ToBox(p);
                p=p+ (Form1->dT)
                      *RotationPhiThetaPsi(Center(Form1->phi[k]),Center(Form1->theta[k]),Center(Form1->psi[k]))
                      *rmatrix(Form1->V[k]);

             }
}

int Contract_Init(void)
{      Form1->NoSolution=false;
       Form1->P=vector<box>(Form1->kmax+1);
       for (int k=0;k<Form1->kmax+1;k++)
          Form1->P[k]=box(interval(-oo,oo),3);
        Form1->P[0]=box(interval(-2.5,2.5),interval(-2.5,2.5),Form1->depth[0]);
        Form1->P[Form1->kmax-1]=box(689.96+interval(-3,3),264.10+interval(-3,3),Form1->depth[Form1->kmax-1]);
        //yfinal = 264.10m, xfinal 689.96m, zfinal=0
}


int Contract_Forward(void)
{       if (Form1->NoSolution) return (-1);
        Form1->Fout <<"\n Je suis dans ContractForward "<<" \n";
        for (int k=0;k<Form1->kmax;k++)
            {   //Form1->Fout<<"k="<<k<<"\n";  //Form1->Fout<<"Form1->depth[k] = "<<Form1->depth[k]<<"\n";   //Form1->Fout<<"Form1->P[k][3] forw = "<<Form1->P[k][3]<<"\n";
                //Form1->P[k][3]=Inter(Form1->P[k][3],Form1->depth[k]);
                Form1->P[k][3]=Form1->depth[k];
                if (Form1->P[k].IsEmpty())
                  {
                     Form1->Fout <<"\n P["<<k<<"] = "<<Form1->P[k]<<" \n";
                     Form1->NoSolution=true;
                     return (-1);
                  }
                Form1->P[k+1]=Inter(Form1->P[k+1],
                                    Form1->P[k]+ToBox((Form1->dT)
                                                      *RotationPhiThetaPsi(Form1->phi[k],Form1->theta[k],Form1->psi[k])
                                                      *imatrix(Form1->V[k])));
            };
       Form1->Fout <<"\n ContractForward s'est terminée correctement "<<" \n";

}

int Contract_Backward(void)
{       if (Form1->NoSolution) return (-1);
        Form1->Fout <<"\n Je suis dans ContractBackward "<<" \n";
        for (int k=Form1->kmax-2;k>=0;k--)
            {
                Form1->P[k]=Inter(Form1->P[k],
                                    Form1->P[k+1]-ToBox((Form1->dT)
                                                      *RotationPhiThetaPsi(Form1->phi[k],Form1->theta[k],Form1->psi[k])
                                                      *imatrix(Form1->V[k])));
                if (Form1->P[k].IsEmpty())
                 { Form1->NoSolution=true;
                  return (-1); };
            };
                   Form1->Fout <<"\n ContractBackward s'est terminée correctement "<<" \n";

}




int Contract_Mine(void)
{
        if (Form1->NoSolution) return (-1);
        Form1->Fout <<"\n Je suis dans ContractMine "<<" \n";
        for (int k=0;k<Form1->kmax;k++)
            {   int kp=Form1->pingNb[k];
                if (kp!=-1)
                  {  int km=Form1->pings[kp].mine;

                     Form1->mines[km].P[3]=Inter(Form1->mines[km].P[3],Form1->P[k][3]+Form1->altitude[k]);
                     Cdistance(Form1->pings[kp].distance,Form1->P[k],Form1->mines[km].P);
                     if ((Form1->pings[kp].distance).isEmpty)
                     {  Form1->NoSolution=true;
                        return -1 ;
                     }

                     imatrix R=RotationPhiThetaPsi(Form1->phi[k],Form1->theta[k],Form1->psi[k]);
                     box ir=Column(R,1);
                     box jr=Column(R,2);
                     box kr=Column(R,3);

                     box ek=Form1->mines[km].P-Form1->P[k];
                     COrtho(ek, ir);
                     Cmoins(ek,Form1->mines[km].P,Form1->P[k],-1);

                     interval a(0.00001,oo);
                     interval b(0.00001,oo);
                     box ajr=a*jr;
                     box bkr=b*kr;
                     Cplus(ek,ajr,bkr,1);
                     Cplus(Form1->mines[km].P,Form1->P[k],ek,1);
                     Cplus(Form1->mines[km].P,Form1->P[k],ek,-1);
                  }
             }
}

int ContractDeuxMines (int kp1,int kp2)
{  int k1=Form1->pings[kp1].k;
   int k2=Form1->pings[kp2].k;
   int km1=Form1->pings[kp1].mine;
   int km2=Form1->pings[kp2].mine;
   int km=km1;
   if (km1!=km2)   return 0;
   interval mx=Form1->mines[km].P[1];
   interval my=Form1->mines[km].P[2];
   interval P1x=Form1->P[k1][1];   interval P1y=Form1->P[k1][2];
   interval P2x=Form1->P[k2][1];   interval P2y=Form1->P[k2][2];

   interval Dx1=P1x-mx;            interval Dy1=P1y-my;
   interval Dx2=P2x-mx;            interval Dy2=P2y-my;

   interval Ux1=Dx1/Form1->pings[kp1].distance;
   interval Uy1=Dy1/Form1->pings[kp1].distance;
   interval Ux2=Dx2/Form1->pings[kp2].distance;
   interval Uy2=Dy2/Form1->pings[kp2].distance;


   interval psi=(Form1->psi[k2]-Form1->psi[k1]);
   if (CAngle(Ux2,Uy2,psi,Ux1,Uy1,true)==-1)
       return -1 ;



   Dx1=Ux1*Form1->pings[kp1].distance;
   Dy1=Uy1*Form1->pings[kp1].distance;
   Dx2=Ux2*Form1->pings[kp2].distance;
   Dy2=Uy2*Form1->pings[kp2].distance;


   Cmoins(Dx1,P1x,mx,-1);
   Cmoins(Dy1,P1y,my,-1);
   Cmoins(Dx2,P2x,mx,-1);
   Cmoins(Dy2,P2y,my,-1);

   Form1->P[k2][2]=P2y;
   Form1->P[k2][1]=P2x;
   Form1->P[k1][2]=P1y;
   Form1->P[k1][1]=P1x;
   Form1->mines[km].P[2]=my;
   Form1->mines[km].P[1]=mx;
};

int Contract_Essai(void)
{       for (int kp1=0;kp1<Form1->pings.size();kp1++)
        for (int kp2=kp2+1;kp2<Form1->pings.size();kp2++)
        {
           ContractDeuxMines(kp1,kp2);
        }
}



