// #define DEBUG_INFO
// #define AUTO_CONFIGURE_XBEE

#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(PIN_EMERGENCY, INPUT);
    emergency_debouncer.attach(PIN_EMERGENCY, INPUT);
    emergency_debouncer.interval(200);

    // ----- 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, 90, 60, 90, 90, 60, 90, 90); // for new whill firmware
    delay(1000);

    Serial.println("check speed profile");

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

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

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

    if (digitalRead(PIN_EMERGENCY) == LOW) b_motor_active = true;
    else                                   b_motor_active = false;

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


void loop()
{
    emergency_debouncer.update();
    if (emergency_debouncer.rose())
    {
        b_motor_active = false;
    }
    if (emergency_debouncer.fell())
    {
        b_motor_active = true;
    }

    setVelocity();

    whill.parse();

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