// #define DEBUG_INFO

#include "global.h"
#include "main.h"

void setup()
{
    Serial.begin(115200);

    delay(2000);

    identify();

    // ----- init Eeprom ------
    // ----- setup XBee -----

    initEeprom(HARDWARE_ID);

    delay(2000);

    initXBee(HARDWARE_ID);
    unpacker.attach(XBEE_SERIAL);

    // ----- Emergency Switch -----

    pinMode(32, INPUT_PULLUP);

    // ----- check I2C -----
    Wire.begin();
    // i2c_Scan(Wire);

    Serial.println("mpu setup");
    mpu.setup(Wire);

    // ----- check LED Driver -----
    Serial.println("IR LED setup");
    irled.setup(Wire);
    pinMode(17, OUTPUT);
    digitalWrite(17, LOW); // Output Enable


    WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
    WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R
    whill.attach(WHILL_SERIAL_L, WHILL_SERIAL_R);

    whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE);
    whill.stop();

    delay(2000);

    // WHILL setting
    Serial.println("set speed profile");

    whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 60, 50, 80, 60, 60, 160); // max
    delay(1000);

    Serial.println("check speed profile");

    whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE); // valid in next streaming phase
    waitReply(WhillOmni::LR::L, WHILL_INFO_INTERVAL_MS + 1000);
    waitReply(WhillOmni::LR::R);

    Serial.println("change streaming to sensor info");

    whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
    delay(1000);


    Serial.println("start program");
}


void loop()
{
    setVelocity();

    whill.parse();
    sendStatus(WhillOmni::LR::L);
    sendStatus(WhillOmni::LR::R);
}
