#include <QDebug>
#include "scout.h"
#include "vibes.h"

extern double dt;
extern double eps;
extern interval Eps;
extern int k;
extern double t;


scout::scout(double x1, double y1, double theta1, double v1)
{   x=x1;y=y1;theta=theta1;v=v1;
    u1=0;u2=-1;
    E=box(interval(-10,10),interval(-10,10));
}
//----------------------------------------------------------------
box scout::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 scout::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],Boatx[k2],Boaty[k2],Dboat[k2],outer);
        X=X&phi(Xk2,k2,k1);
    }
//----------------------------------------------------------------
void scout::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 scout::CdistBuddy(box& X)
{
  box A=Ebuddy;
  interval R=Dbuddy;
  Cdist(R, X[1],X[2],A[1],A[2]);
  return;
}

//----------------------------------------------------------------
void scout::Contract(box& X, int k1)
{   SepObsk1(X,k1,1);
    CdistBuddy(X);
}
//----------------------------------------------------------------
void scout::ClockMove()
{   double x1,y1,theta1,v1;
    x1      = x   + dt*v*cos(theta);
    y1      = y   + dt*v*sin(theta);
    v1      = v   + dt*(-0.1*v*v+u1);
    theta1  = theta + dt*u2;
    x=x1; y=y1; theta=theta1; v=v1;
    qDebug()<<"v="<<v;
}
//----------------------------------------------------------------
void scout::ClockStore(double boatx,double boaty, interval Dboat1)
{   Dboat.push_back(Dboat1);
    Theta.push_back(theta+Rand(Eps)+Eps);
    Boatx.push_back(boatx);
    Boaty.push_back(boaty);
    V.push_back(v+Rand(Eps)+Eps);
}
//----------------------------------------------------------------
void scout::Sivia(interval Dbuddy1, const box& Ebuddy1)
{   Ebuddy=Ebuddy1;  Dbuddy=Dbuddy1;
    if (k==0) return;
    box X(E);
    E=EmptyBox(2);
    list<box> L;
    int h=0;
    L.push_back (X);
    while ( !L.empty() )
    {   h++; X=L.front();   L.pop_front();
        Contract(X,k);
        if (h<10)
        {   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]");
            E=E|X;
        }
    }
    vibes::drawBox(E[1].inf,E[1].sup,E[2].inf,E[2].sup,"black");
}
//----------------------------------------------------------------
void scout::ClockNextE()
{   E=phi(E,k,k+1);   E.DrawBox("green");}
//----------------------------------------------------------------
void scout::ClockControl(double Bx, double By, double Btheta, double Bthetadot, double Bv,double qxB,double qyB)
{   // qxB et et qyB : target in the Boat frame
    double px=Center(E[1]);  double py=Center(E[2]);  //scout in the world frame
    double qx=Bx+cos(Btheta)*qxB-sin(Btheta)*qyB;  // target in the world frame
    double qy=By+sin(Btheta)*qxB+cos(Btheta)*qyB;
    DrawSquare(qx,qy,0.8,0.1,"red"); // setpoint in the world frame
    double ex=qx-px; //error in the world frame
    double ey=qy-py;
    double qdotx=Bv*cos(Btheta)-Bthetadot*(qy-By);
    double qdoty=Bv*sin(Btheta)+Bthetadot*(qx-Bx);
    double wx=qdotx+0.2*ex; // wanted direction
    double wy=qdoty+0.2*ey;
    //DrawArrow(px,py,wx,wy,0.1,"green");
    double vbar=hypot(wx,wy);
    u1=atan(vbar-v);
    qDebug()<<"u1="<<u1;
    double thetabar=atan2(wy,wx);
    u2=atan(tan((thetabar-theta)/2));
    if (Width(E)>2) u1=1;
}


