#include <QDebug>
#include "vibes.h"
#include "interval.h"
#include "box.h"
#include "boat.h"
#include "scout.h"
#include <list>

using namespace std;

double dt=0.1;
double eps=0.03; // noise
interval Eps=interval(-eps,eps);
int k;
double t;

boat B;
scout S1(2,0,3,1);
scout S2(-1,2,0,2);

//----------------------------------------------------------------
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]");

    k=0;
    for (t=0;t<100;t=t+dt)
    {
       //if (k%100==0) vibes::clearFigure();
       double d12=hypot(S1.x-S2.x,S1.y-S2.y)+Rand(Eps);
       double d02=hypot(B.x-S2.x,B.y-S2.y)+Rand(Eps);
       double d01=hypot(B.x-S1.x,B.y-S1.y)+Rand(Eps);

       S1.ClockStore(B.x,B.y,d01+Eps);
       S2.ClockStore(B.x,B.y,d02+Eps);
       for (int a=0;a<3;a++)
       {   S1.Sivia(d12+Eps,S2.E);
           S2.Sivia(d12+Eps,S1.E);
       }
       DrawRobot(B.x,B.y,B.theta,0.1,"red[red]");
       DrawRobot(S1.x,S1.y,S1.theta,0.05,"black");
       DrawRobot(S2.x,S2.y,S2.theta,0.05,"black");
       B.Clock();
       S1.ClockControl(B.x,B.y,B.theta,B.u2,B.v,3,2);
       S2.ClockControl(B.x,B.y,B.theta,B.u2,B.v,4,-2);
       S1.ClockMove();
       S2.ClockMove();
       S1.ClockNextE();
       S2.ClockNextE();
       k++;
    }
    vibes::endDrawing();
}
