#include <PID_v1.h>
#include "qtr8a.h"
#include "tb6612.h"

//#define DEBUG

using namespace iclass;

enum class State
{
    STANDBY,
    RUN,
    OFFLINE,
    LINEUP,
    TIMEOVER,
    STOP
};

const int       LINEWIDTH( 50 );        // mm
const int       THRESHOLD( 550 >> 2 );  // 0..255 for 8 bit resolution
const int       MAXOFFLINE( 700 );      // milliseconds
const int       MAXRUNTIME( 20 );       // seconds
const uint8_t   BUTTONPIN( 11 );
const uint8_t   QTRONPIN( 15 );
const int       MAXPWM( 2047 );
const int       RUNPWM( 655 );
const int       STARTPWM( 350 );
const double    KP( 18.0 );
const double    KI( 0.0 );
const double    KD( 3.3 );


Qtr8a           qtr8a( A2, A3, A4, A5, A6, A7, A8, A9, LINEWIDTH, THRESHOLD );
Tb6612          driver( 7, 6, 5, 4, 9, 8, 10 );
double          pidSetpoint( 0 );
double          pidInput( 0 );
double          pidOutput( 0 );
PID             pid( &pidInput, &pidOutput, &pidSetpoint, KP, KI, KD, REVERSE );

void setup()
{
    analogReadResolution( 8 );
    analogWriteResolution( 11 );
    analogWriteFrequency( 4, 23437.5 );
    analogWriteFrequency( 10, 23437.5 );
    Serial.begin( 230400 );

    pinMode( BUTTONPIN, INPUT_PULLUP );
    pinMode( QTRONPIN, OUTPUT );
    pinMode( LED_BUILTIN, OUTPUT );

    ::pid.SetSampleTime( 1 );
    ::pid.SetOutputLimits( RUNPWM - MAXPWM, MAXPWM - RUNPWM );
    ::pid.SetMode( AUTOMATIC );

    digitalWrite( QTRONPIN, LOW );
    digitalWrite( LED_BUILTIN, HIGH );

    delay( 100 );
}

void loop()
{
    unsigned long           usec( micros() );
    static State            state( State::STANDBY );
    static unsigned long    startTime( 0 );
    static unsigned long    offlineTime( 0 );
    static int              meanPwm( STARTPWM );
    static int              leftPwm( 0 );
    static int              rightPwm( 0 );
    static int              devi( 0 );

    if ( startTime && (usec - startTime) / 1000000 >= MAXRUNTIME )
    {
        state = State::TIMEOVER;
    }

    switch ( state )
    {
        case State::STANDBY: 

            if ( !digitalRead(BUTTONPIN) ) // start
            {
                state = State::RUN;
                digitalWrite( QTRONPIN, HIGH );
                delay( 200 ); // time to hands off
                startTime = micros();
                ::driver.start();
                leftPwm = meanPwm;
                rightPwm = meanPwm;
            }

            delay( 20 ); // power saving
    
            return;

        case State::RUN:

            if ( !digitalRead(BUTTONPIN) ) // stop
            {
                state = State::STOP;

                return;
            }

            if ( usec - startTime > 300000 )
            {
                meanPwm = RUNPWM;
            }

            ::qtr8a.read();

            if ( ::qtr8a.isEmpty() )
            {
                state = State::OFFLINE;

                return;
            }

            devi = ::qtr8a.deviation();
            ::pidInput = devi;
            ::pid.Compute();
            leftPwm = meanPwm + ::pidOutput;
            rightPwm = meanPwm - ::pidOutput;

            break;

        case State::OFFLINE:

            if ( !digitalRead(BUTTONPIN) ) // stop
            {
                state = State::STOP;

                return;
            }

            ::qtr8a.read();

            if ( !::qtr8a.isEmpty() )
            {
                state = State::LINEUP;

                return;
            }

            if ( offlineTime && (usec - offlineTime) / 1000  >= MAXOFFLINE )
            {
                state = State::STOP;
                offlineTime = 0;

                return;
            }

            if ( devi < -5 )
            {
                leftPwm = 0;
                rightPwm = meanPwm;
            }
            else if ( devi > 5 )
            {
                leftPwm = meanPwm;
                rightPwm = 0;
            }
            else
            {
                leftPwm = meanPwm;
                rightPwm = meanPwm;
            }

            break;

        case State::LINEUP:

            ::qtr8a.read();

            if ( ::qtr8a.isEmpty() )
            {
                devi = -devi;
                state = State::OFFLINE;

                return;
            }
            else if ( (devi < -5 && ::qtr8a.rightEdge() < 14) || (devi > 5 && ::qtr8a.leftEdge() > -14) )
            {
                state = State::RUN;

                return;
            }
            
            leftPwm = meanPwm;
            rightPwm = meanPwm;

            break;

        case State::TIMEOVER:
        case State::STOP:

            state = State::STANDBY;
            startTime = 0;
            digitalWrite( QTRONPIN, LOW );
            ::driver.setPwmA( 0 );
            ::driver.setPwmB( 0 );
            delay( 200 );
            ::driver.stop();
    }

    ::driver.setPwmA( leftPwm );
    ::driver.setPwmB( rightPwm );

#ifdef DEBUG

    Serial.print( usec );
    Serial.print( "\t" );
    Serial.print( devi );
    Serial.print( "\t" );
    Serial.print( leftPwm );
    Serial.print( "\t" );
    Serial.println( rightPwm );
  
#endif
}
