#include <ctime>
#include <cmath>
#include <deque>
#include <kovan/motor.hpp>

#define LMOTOR 0
#define RMOTOR 1
#define TOPHAT 6
#define GRAYLOW 256
#define GRAYHIGH 767
#define WIDTH 250
//#define OFFSET .5
#define BIAS 5
#define ACC 3
#define PREC 5
#define DEBUG_LINEFOLLOW 1

bool lineFollow(int distance, int speed)
{
    Motor lmotor(LMOTOR);
    Motor rmotor(RMOTOR);
    Analog irsens(TOPHAT);
    int timeorig=std::time(NULL), counter=0, turn=0, tmptime=seconds() * 1000;
    std::deque<double> alpha(ACC);
    double theta, omega, timetotal=distance/speed, tmpa=0;
    lmotor.MoveAtVelocity(speed);
    while(seconds()-timeorig <= timetotal)
    {
        if(counter%PREC==0)
        {
            counter=0;
            if(irsens.value()>GRAYHIGH)
            {
                for(std::deque<double>::iterator i=alpha.begin(); i!=alpha.end() && i<alpha.end(); i++)
                {
                    tmpa += *i;
                }
                alpha.pop_front();
                alpha.push_back(tmpa/ACC+0.25);
                tmpa=0;
            }
            else if(irsens.value()<GRAYLOW)
            {
                for(std::deque<double>::iterator i=alpha.begin(); i!=alpha.end() && i<alpha.end(); i++)
                {
                    tmpa += *i;
                }
                alpha.pop_front();
                alpha.push_back(tmpa/ACC-0.25);
                tmpa=0;
            }
            else
            {
                theta=0;
                turn=0;
            }
            omega += alpha.back();
            if (omega>0)
            {
                rmotor.MoveAtVelocity(speed-WIDTH*tan(omega));
                lmotor.MoveAtVelocity(speed+BIAS);
            }
            else
            {
                lmotor.MoveAtVelocity(speed-WIDTH*tan(-1*omega)
                rmotor.MoveAtVelocity(speed+BIAS);
            }
            omega>0 ? rmotor.MoveAtVelocity(speed-WIDTH*tan(omega)) : lmotor.MoveAtVelocity(speed-WIDTH*tan(-1*omega));
            theta += omega.back()*(seconds() * 1000-tmptime);
            tmptime=seconds() * 1000.0;
        }
        switch(turn)
        {
        case 0:
            if(theta > 15)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()*-1);
                turn++;
            }
            break;
        case 1:
            if(theta < -15)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()+2);
                turn++;
            }
            break;
        case 2:
            if (theta < -170)
            {
                alpha.pop_front();
                alpha.push_back(alpha.back()*-1);
                turn++;
            }
            break;
        case 3:
            if (theta > 170)
            {
                return 0;
            }
        }
        counter++;
        if (seconds() * 1000.0 >= tmptime)
        {
            theta+=omega.back();
        }
        msleep(5);
    }
    return 1;
}
#ifdef DEBUG_LINEFOLLOW
int main()
{
    Motor lmotor(LMOTOR);
    Motor rmotor(RMOTOR);
    lmotor.MoveRelativePosition(100, 200);
    rmotor.MoveRelativePosition(500, 1000);
    lmotor.BlockMotorDone();
    return lineFollow(2000, 500);
}
#endif