// #define DEBUG_INFO
// #define AUTO_CONFIGURE_XBEE

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

void disableMotor() { b_motor_active = false; }

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(PIN_EMERGENCY, 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(PIN_IRLED_OE, OUTPUT);
    digitalWrite(PIN_IRLED_OE, 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(500);

    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 + 500);
    waitReply(WhillOmni::LR::R);

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

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

    attachInterrupt(digitalPinToInterrupt(PIN_EMERGENCY), disableMotor, FALLING);

    Serial.println("start program");
    fps.start();
}


void loop()
{
    setVelocity();

    whill.parse();

    static uint32_t prev_ms = millis();
    if ((millis() - prev_ms) > streaming_interval)
    {
        sendStatus();
        prev_ms = millis();
    }
}
