#include "vibes.h"
#include "interval.h"
#include "box.h"
#include <list>
using namespace std;
//   more more explanation, see : https://www.ensta-bretagne.fr/jaulin/easibex.html  Example 3

vector<interval> V,Theta,D;  // V:speed, Theta: heading; D: distance
vector<double> Xr,Yr;
double dt=0.1;
double eps=0.01; // noise
interval Eps=interval(-eps,eps);
int kmax;

//----------------------------------------------------------------
box phi(const box&X,int k1, int k2)
{  if (k1==k2) return X;
   if (k2>k1) return(phi(X,k1,k2-1)+dt*V[k2-1]*box(cos(Theta[k2-1]),sin(Theta[k2-1])));
   if (k2<k1) return(phi(X,k1,k2+1)-dt*V[k2]*box(cos(Theta[k2]),sin(Theta[k2])));
}
//----------------------------------------------------------------
void SepObsk1k2(box& X, int k1, int k2, bool outer) // to be consistent with the data at time k2
    {   box Xk2(2);
        Xk2=phi(X,k1,k2);
        SinRing(Xk2[1],Xk2[2],0,0,D[k2],outer);
        X=X&phi(Xk2,k2,k1);
    }
//----------------------------------------------------------------
void SepObsk1(box& X, int k1, bool outer) // consistent with a past window of length 5
{  vector<box> Xk;
   for (int k2=k1; (k2>0)&&(k1-k2<5);k2--)
    {  box Xk1(X);
       SepObsk1k2(Xk1,k1,k2,outer);
       Xk.push_back(Xk1);
    }
    S_q_in(X,Xk.size(),Xk,outer);
}
//----------------------------------------------------------------
void sivia(int k)
{   box X(interval(-10,10),interval(-10,10));
    list<box> L;
    int h=0;
    L.push_back (X);
    while ( !L.empty() )
    {   h++; X=L.front();   L.pop_front();
        SepObsk1(X,k,1);
        SepObsk1(X,k,0);
        if (h<2000)
        {   box X1(2);  box X2(2);
            Bisect (X,X1,X2);
            L.push_back(X1);  L.push_back(X2);
        }
        else vibes::drawBox(X[1].inf,X[1].sup,X[2].inf,X[2].sup,"black[yellow]");
    }
}
//----------------------------------------------------------------
int main()
{   vibes::beginDrawing();
    vibes::newFigure("Observer");
    vibes::setFigureProperties(vibesParams("x",20,"y",40,"width",1500,"height",700));
    vibes::clearFigure();
    vibes::drawCircle(0,0,0.1,"red[red]");
    for (double t=0;t<5;t=t+dt)
    {  double xr=2+3*cos(t);  double yr=sin(t);
       vibes::drawCircle(xr,yr,0.01,"blue[blue]");
       Xr.push_back(xr);    Yr.push_back(yr);
       double d1=hypot(xr,yr)+Rand(Eps);
       D.push_back(d1+Eps);
    }
    for (int k=0;k<Xr.size()-1;k++)
    {  double dxr=(Xr[k+1]-Xr[k])/dt;
       double dyr=(Yr[k+1]-Yr[k])/dt;
       Theta.push_back(atan2(dyr,dxr)+Rand(Eps)+Eps);
       V.push_back(hypot(dyr,dxr)+Rand(Eps)+Eps);
   }
   kmax=V.size();
   for (int k=1;k<=kmax;k++)
   { sivia(k);
     vibes::drawCircle(Xr[k],Yr[k],0.01,"green[green]");
   }
    vibes::endDrawing();
}
