//#DOC Main, program entry and main loop
#include "main.h"
#include <otMath.h>
#include <otSerial.h>
#include <otTimer.h>
#include <otMalloc.h>
#include <otPort.h>
#include <otPrintf.h>
#include <otI2cBus.h>
#include <otMath.h>
#include <otFPGA.h>
#include <otPWM.h>
#include <otRFCom.h>
#include <otTFMini.h>
#include <otWS2812.h>
#include <otBotEngine.h>

// These parameters are related to the model used in the test
#define TrackWidth  265             // mm

// The underlying value was found experimentally using a LIDAR
#define EncoderStep 0.737463126f    // mm x encoder count

#define Pi          3.14159265359f

// Convert a length to the number of encoder counts
#define Travel(x)   ((float) (x) / EncoderStep)

// Space traveled for 1 degree rotation
#define AngleUnit   (Pi * TrackWidth / 360.0f)

// Calculate the number of encoder counts based on the desired angle.
// Valid for rotation carried out with both motors.
#define Angle(x) Travel(((float) (x) * AngleUnit / 2.0f))

U16     _distance;

otPWM       pwm;
otRFCom     rf;
otTFMini    tof;
otWS2812    ledPanel;
otBotEngine bot;

void setup()
{
    UartEnableSignals(UART0);
    UartSetBaud(UART0, 115200);
    
    TimerSetAsCounter(TIMER0);
    TimerEnable(TIMER0, true);
    
    // Heap init
    _initMalloc();
    // printf output is now directed on UART0
    setStdout(4096, UART0);
    
    I2cBusReset();
    I2cBusFrequency(100, 30);
    
    if(pwm.init() == false)
    {
        printf("Error on PWM init\n");
        IDLE;
    }
    pwm.setTimebase(25000000);
    pwm.setPWMFreq(50);
    
    bot.init(TIMER0, &pwm);
    bot.setBrakeTime(20);
    
    rf.init(UART0, PP_H, 4);
    rf.setBaud(otRFCom::B_115200);
    rf.apply();
    
    ledPanel = otWS2812(12, 16, 16);
    ledPanel.mirror(true, true);
    ledPanel.setOffset(1);
    
    tof.init(UART1);
}

void loop()
{
    bool    ok;

    ledPanel.rectFill(0, 0, 15, 15, 5, 0, 0);
    ledPanel.show();
    DelayMs(1000);
    ledPanel.rectFill(0, 0, 15, 15, 0, 0, 0);
    ledPanel.show();
    
    while(1)
    {
        if(UartCheckChar(UART0))
        {
            switch(UartChar(UART0))
            {
                case 'F':   bot.action(otBotEngine::MA_FORWARD);
                            break;
                case '>':   bot.action(otBotEngine::MA_FORWARD, Travel(100));
                            break;
                case 'B':   bot.action(otBotEngine::MA_BACKWARD);
                            break;
                case '<':   bot.action(otBotEngine::MA_BACKWARD, Travel(100));
                            break;
                case 'L':   bot.action(otBotEngine::MA_TURNLEFT);
                            break;
                case '[':   bot.action(otBotEngine::MA_TURNLEFT, Angle(10));
                            break;
                case 'R':   bot.action(otBotEngine::MA_TURNRIGHT);
                            break;
                case ']':   bot.action(otBotEngine::MA_TURNRIGHT, Angle(10));
                            break;
                case '.':   bot.action(otBotEngine::MA_BRAKE);
                            break;
                case '?':   printf("Sx = %5d, Dx = %5d\n", bot.encoder(true), bot.encoder(false));
                            break;
                case 'D':   _distance = tof.getDistance(&ok);
                            if(ok)
                                printf("Dx = %5d cm\n", _distance);
                            break;
                default:
                            printf("Unkonwn command!\n");
            }
        }
        bot.process();
    }
}