#include "mainwindow.h"
#include "ui_mainwindow.h"


double xmin,xmax,ymin,ymax;
interval P1,P2,P3;


MainWindow::MainWindow(QWidget *parent) :  QMainWindow(parent),
      ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    xmin=-10;xmax=10;
    ymin=-10;ymax=10;
    Rrobot=new repere(this,ui->graphicsViewRobot,xmin,xmax,ymin,ymax);
    Rtraj=new repere(this,ui->graphicsViewTraj,xmin,xmax,ymin,ymax);
    Rmap=new repere(this,ui->graphicsViewMap,xmin,xmax,ymin,ymax);
    Rpose=new repere(this,ui->graphicsViewPose,xmin,xmax,ymin,ymax);
    P0=box(2);
    P0[1]=interval(-10,10);	P0[2]=interval(-10,10);
    noise=interval(-0.0002,0.0002);
    Map=paving(P0,iperhaps);
    testcase=2;
    eps=0.4;
    DrawRobot(Rrobot,1.0,2.0,1.0);
    Readdata();
    DrawRoom(Rrobot);
}



MainWindow::~MainWindow()
{
    delete ui;
}


void MainWindow::DrawRobot(repere* R, double x,double y,double theta)
{       vector<double> X,Y;
        double a=0.2;
        X.push_back(-a); Y.push_back(-a);
        X.push_back(2*a);  Y.push_back(0);
        X.push_back(-a);  Y.push_back(a);
        X.push_back(-a); Y.push_back(-a);
        R->DrawPolygone(x,y,theta,X,Y,QPen(Qt::magenta),QBrush(Qt::red));
}

void MainWindow::DrawRoom(repere* R)
{   for (int j=0;j<N_cercles;j++)
    {   R->DrawEllipse(cercles_x[j],cercles_y[j],cercles_r[j],QPen(Qt::green),QBrush(Qt::green));
    }
    for (int j=0;j<N_murs;j++)
    {   vector<double> X,Y;
        X.push_back(murs_xa[j]); Y.push_back(murs_ya[j]);
        X.push_back(murs_xb[j]); Y.push_back(murs_yb[j]);
        R->DrawPolygone(0,0,0,X,Y,QPen(Qt::green),QBrush(Qt::green));
    }
} ;

void MainWindow::DrawMap(repere* R)
{  R->Clean();
   for (int i=0;i<Map.B.size();i++)
   { if (Map.left[i]==-1)
        {
           if (Map.val[i]==iperhaps) {R->DrawBox(Map.B[i][1],Map.B[i][2],QPen(Qt::yellow),QBrush(Qt::yellow));}
                //addBox(Map.B[i],QColor("Yellow"));};
            if (Map.val[i]== ifalse)  {R->DrawBox(Map.B[i][1],Map.B[i][2],QPen(Qt::blue),QBrush(Qt::cyan));};
            if (Map.val[i]== itrue)   {R->DrawBox(Map.B[i][1],Map.B[i][2],QPen(Qt::red),QBrush(Qt::magenta));};
            if (Map.val[i]== empty)   {R->DrawBox(Map.B[i][1],Map.B[i][2],QPen(Qt::black),QBrush(Qt::black));};
        }
   }
   DrawRoom(R);
   repaint();
}


void MainWindow::DrawTraj(repere* R,QPen pen1, QBrush brush1)
{   for (int k=0;k<k_max-1;k++)
    {   R->DrawBox(X[k],Y[k],pen1,brush1);
    }
    DrawRoom(R);
} ;


void MainWindow::Readdata()
{   qDebug() << "readdata ";
    X.clear();Y.clear();Theta.clear();V.clear();
    x.clear();y.clear();theta.clear();v.clear();u1.clear(); u2.clear();d.clear();;
    cercles_x.clear();cercles_y.clear();cercles_r.clear();;
    murs_xa.clear();murs_xb.clear();murs_ya.clear();murs_yb.clear();
    QString name_file;
    if (testcase==1) name_file="data1.txt" ;
    if (testcase==2) name_file="data2.txt" ;
    QFile file(name_file);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) return;

    QTextStream in(&file);
    QString line1 = in.readLine();
    QStringList mots;
    mots = line1.split(QRegExp("\\s+"));
    QString str=mots.takeFirst();
    N_cercles=str.toInt();
    qDebug() << "N_cercles = "<<N_cercles<<'\n';

    for (int i=0;i<N_cercles;i++)
    {  double cercles_x1,cercles_y1,cercles_r1;
       line1 = in.readLine();
       mots = line1.split(QRegExp("\\s+"));
       str=mots[0];   cercles_x1=str.toDouble();
       str=mots[1];   cercles_y1=str.toDouble();
       str=mots[2];   cercles_r1=str.toDouble();
       cercles_x.push_back(cercles_x1);cercles_y.push_back(cercles_y1);cercles_r.push_back(cercles_r1);
    }
    line1 = in.readLine();
    mots = line1.split(QRegExp("\\s+"));
    str=mots[0];
    N_murs=str.toInt();
    for (int i=0;i<N_murs;i++)
        {  double murs_xa1,murs_xb1,murs_ya1,murs_yb1;
           line1 = in.readLine();
           mots = line1.split(QRegExp("\\s+"));
           str=mots[0];   murs_xa1=str.toDouble();
           str=mots[1];   murs_ya1=str.toDouble();
           str=mots[2];   murs_xb1=str.toDouble();
           str=mots[3];   murs_yb1=str.toDouble();
           murs_xa.push_back(murs_xa1);murs_ya.push_back(murs_ya1);murs_xb.push_back(murs_xb1);murs_yb.push_back(murs_yb1);
        }

    line1 = in.readLine();
    while (!in.atEnd())
     {   int k1;
         double x1,y1,theta1,v1,u11,u21,d1;
         line1 = in.readLine();
         mots = line1.split(QRegExp("\\s+"));
         str=mots[1];   k1=str.toInt();
         str=mots[2];   x1=str.toDouble();
         str=mots[3];   y1=str.toDouble();
         str=mots[4];   theta1=str.toDouble();
         str=mots[5];   v1=str.toDouble();
         str=mots[6];   u11=str.toDouble();
         str=mots[7];   u21=str.toDouble();
         str=mots[8];   d1=str.toDouble();
         x.push_back(x1);y.push_back(y1);theta.push_back(theta1);v.push_back(v1);
         u1.push_back(u11);u2.push_back(u21);d.push_back(d1);
         X.push_back(interval(-oo,oo));Y.push_back(interval(-oo,oo));Theta.push_back(theta1+noise);V.push_back(v1+noise);
     }
    k_max=x.size();
    X[0]=interval(x[0]);  // the initial condition is known
    Y[0]=interval(y[0]);
}

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

void MainWindow::Contract_Traj()
{   Rtraj->Clean();
    //R_error.Clean();
    qDebug() << "k_max = "<<k_max<<" = "<<X.size()<<'\n';
    for (int k=0;k<k_max-1;k++)
     { X[k+1]   = Inter(X[k+1],X[k]+ 0.1*(Cos(Theta[k])*V[k])+noise);
       Y[k+1]   = Inter(Y[k+1],Y[k]+ 0.1*(Sin(Theta[k])*V[k])+noise);
       //box XY=box(X[k],Y[k]);
       //double w=XY.SumWidth();
       //if (w>0) R_error.DrawPoint(k,w,clBlack,1);
       //if ((1.0*k/50)==floor(k/50))
          //{R_traj.image->Refresh(); R_error.image->Refresh();};
     }
    DrawTraj(Rtraj,QPen(Qt::gray),QBrush(Qt::gray));

    for (int k=x.size()-2;k>0;k--)
     { //Draw_robot(R3,x[k],y[k],theta[k]);
       X[k]=Inter(X[k],X[k+1]- 0.1*(Cos(Theta[k])*V[k])+noise)   ;
       Y[k]=Inter(Y[k],Y[k+1]- 0.1*(Sin(Theta[k])*V[k])+noise)   ;
       //box XY=box(X[k],Y[k]);
       //double w=XY.SumWidth();
       //if (w>0) R_error.DrawPoint(k,w,clDkGray,1);
       //Rtraj->DrawBox(X[k],Y[k],QPen(Qt::black),QBrush(Qt::black));
       //R_traj.DrawCadre(XY,clDkGray,psSolid);
       //if ((1.0*k/50)==floor(k/50))
        //{R_traj.image->Refresh(); R_error.image->Refresh();};
     }
    for (int k=0;k<k_max-1;k++)
      {    //R_traj.DrawPoint(x[k],y[k],clBlack,1);
           X[k]=Inter(X[k],P0[1])   ;
           Y[k]=Inter(Y[k],P0[2])   ;
      }

    DrawTraj(Rtraj,QPen(Qt::black),QBrush(Qt::black));
    //R_traj.image->Refresh();
    //Image2->Picture->SaveToFile("Im_traj_"+String(100000+k_file)+".bmp"); k_file++;
    //Image5->Picture->SaveToFile("Im_error_"+String(100000+k_file)+".bmp"); k_file++;
}

iboolean ThickDisk(const box& P)
{   iboolean dedans;
    interval Ym=Sqr(P[1])+Sqr(P[2]);
    if (Subset(Ym,Sqr(P1))) return itrue ;
    if (Subset(Ym,Sqr(P2))) return ifalse ;
    return iperhaps;
};

iboolean DiskForall(const box& X)
{   // Test for the constraint :  Forall p1 in P1, p2  in P2, (x1-p1)^2+(x2-p2)^2 in P3
    iboolean dedans1,dedans2;
    interval A1=X[1]-P1.inf;
    interval B1=X[1]-P1.sup;
    interval A2=X[2]-P2.inf;
    interval B2=X[2]-P2.sup;
    interval A12=Sqr(A1);
    interval A22=Sqr(A2);
    interval B12=Sqr(B1);
    interval B22=Sqr(B2);
    interval Z1=Step(B1*A1)*Min(B12,A12)+Step(B2*A2)*Min(B22,A22);
    interval Z2=Max(B12,A12)+Max(B22,A22);
    dedans1=In(Z2-P3.sup*P3.sup,interval(-oo,0));
    dedans2=In(Z1-P3.inf*P3.inf,interval(0,oo));
    return (dedans1&&dedans2);
};


iboolean DiskExists(const box& X)
{   // Test for the constraint :  Exists p1 in P1, p2  in P2, (x1-p1)^2+(x2-p2)^2 in P3
    iboolean dedans;
    interval A1=X[1]-P1.inf;
    interval B1=X[1]-P1.sup;
    interval A2=X[2]-P2.inf;
    interval B2=X[2]-P2.sup;
    interval A12=Sqr(A1);
    interval A22=Sqr(A2);
    interval B12=Sqr(B1);
    interval B22=Sqr(B2);
    interval Z1=Step(B1*A1)*Min(B12,A12)+Step(B2*A2)*Min(B22,A22);
    interval Z2=Max(B12,A12)+Max(B22,A22);

    dedans=In(Min(Z2,P3.sup*P3.sup)-Max(Z1,P3.inf*P3.inf),interval(0,oo));
    return (dedans);
};




iboolean Restrictbis(iboolean x, iboolean y)      // return x and !y
{ return And(x,!y);}



void MainWindow::Contract_Map()
{   //R_map.Clean();
    //R1.Clean();
    //DrawRoom(R1);
    int dk=10;
    //int cpt=0;
    int k0= floor((1.0*rand()/RAND_MAX)*dk);
    for (int k=k0;k<k_max;k=k+dk)
     { //TrackBar1->Position=k;
       P1=X[k];P2=Y[k];P3=interval(0,d[k]);
       Map.Sivia(DiskForall,Restrictbis,eps);
       //R_map.DrawPaving(Map);
       //Draw_robot(R1,x[k],y[k],theta[k]);
       //DrawRoom(R_map);
       //R1.image->Refresh(); R_map.image->Refresh();
     }
    //Image3->Picture->SaveToFile("Im_map_"+String(100000+k_file)+".bmp");  k_file++;
}


void MainWindow::Contract_Pose(int k)
{   box Q(X[k],Y[k]);
    Contract_distance_gt_paving(Map, d[k], Q);
    X[k]=Q[1];
    Y[k]=Q[2];
} ;

void MainWindow::Contract_All_Poses()
{  Rpose->Clean();
   list<box> L1;
   list<box> L2;
    int dk=5;
    int k0=floor((1.0*rand()/RAND_MAX)*dk);
    for (int k=k0;k<k_max;k=k+dk)
      {   box XY1=box(X[k],Y[k]);
          Contract_Pose(k);
          box XY2=box(X[k],Y[k]);
          if (EloignementRelatif2(XY1,XY2)>0.01)
          { L1.push_back(XY1);
            //R_pose.DrawBox(XY1,clBlack,clBlack,bsSolid);
            Rpose->DrawBox(XY1[1],XY1[2],QPen(Qt::black),QBrush(Qt::black));
            L2.push_back(XY2);
            Rpose->DrawBox(XY2[1],XY2[2],QPen(Qt::gray),QBrush(Qt::gray));
            //double w=XY2.SumWidth();
            //if (w>0)  R_error.DrawPoint(k,w,clBlack,2);
            //R_error.image->Refresh();
          }
          //TrackBar1->Position=k;
      }
   //R_pose.Clean();
    while (!L1.empty())
   { box Q=L1.front();
     L1.pop_front();
     //R_pose.DrawBox(Q,clBlack,clBlack,bsSolid);
    }
    while (!L2.empty())
   { box Q=L2.front();
     L2.pop_front();
     //R_pose.DrawBox(Q,clLtGray,clLtGray,bsSolid);
    }
     //DrawRoom(R_pose);
     //R_error.image->Refresh();
     //R_pose.image->Refresh();
     //Image4->Picture->SaveToFile("Im_pose"+String(100000+k_file)+".bmp"); k_file++;
    qDebug() << "End ";

}





void MainWindow::on_ButtonStart_clicked()
{            Contract_Traj();
             Contract_Map();
             DrawMap(Rmap);
             Contract_All_Poses();
             //DrawTraj(Rtraj,QPen(Qt::yellow),QBrush(Qt::yellow));
             eps=0.5*eps;
}


