#pragma once
#ifndef MAIN_HEADER_H
#define MAIN_HEADER_H

#include <Arduino.h>
#include <WhillOmni.h>
#include <BorderXBeePacketizer.h>
#include <StopWatch.h>
#include <XBeeATCmds.h>
#include <MPU9250.h>
#include <PCA9624.h>
#include <RzmEepromMap.h>
#include <Bounce2.h>

using namespace Whill;

WhillOmni whill;
BorderXBeePacker packer;
BorderXBeeUnpacker unpacker;
XBeeATCmds xbeecmd;
MPU9250 mpu;
PCA9624 irled(0x60);
RzmEepromMap eepmap;
StopWatch stopwatch;
FrameRateCounter fps(30);
Bounce emergency_debouncer;

void printSpeedProfile(WhillOmni::LR lr)
{
    Serial.println("SPEED_PROFILE");
    Serial.print("mode    : "); Serial.println(whill.speedMode(lr));
    Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed(lr));
    Serial.print("fwd acc : "); Serial.println(whill.forwardAccel(lr));
    Serial.print("fwd dec : "); Serial.println(whill.forwardDecel(lr));
    Serial.print("bwd spd : "); Serial.println(whill.reverseSpeed(lr));
    Serial.print("bwd acc : "); Serial.println(whill.reverseAccel(lr));
    Serial.print("bwd dec : "); Serial.println(whill.reverseDecel(lr));
    Serial.print("trn spd : "); Serial.println(whill.turnSpeed(lr));
    Serial.print("trn acc : "); Serial.println(whill.turnAccel(lr));
    Serial.print("trn dec : "); Serial.println(whill.turnDecel(lr));
}

void printSensorInfo(WhillOmni::LR lr)
{
    Serial.println("SENSOR_INFO");
    Serial.print("sensor acc x : "); Serial.println(whill.sensorAccelX(lr));
    Serial.print("sensor acc y : "); Serial.println(whill.sensorAccelY(lr));
    Serial.print("sensor acc z : "); Serial.println(whill.sensorAccelZ(lr));
    Serial.print("sensor gyr x : "); Serial.println(whill.sensorGyroX(lr));
    Serial.print("sensor gyr y : "); Serial.println(whill.sensorGyroY(lr));
    Serial.print("sensor gyr z : "); Serial.println(whill.sensorGyroZ(lr));
    Serial.print("joystick fb  : "); Serial.println(whill.joystickFront(lr));
    Serial.print("joystick lr  : "); Serial.println(whill.joystickSide(lr));
    Serial.print("battery      : "); Serial.println(whill.battery(lr));
    Serial.print("battery crnt : "); Serial.println(whill.batteryCurrent(lr));
    Serial.print("angle rad r  : "); Serial.println(whill.angleRadR(lr));
    Serial.print("angle rad l  : "); Serial.println(whill.angleRadL(lr));
    Serial.print("speed r      : "); Serial.println(whill.speedR(lr));
    Serial.print("speed l      : "); Serial.println(whill.speedL(lr));
    Serial.print("power state  : "); Serial.println(whill.powerState(lr));
    Serial.print("speed mode   : "); Serial.println(whill.speedModeIndicator(lr));
    Serial.print("error code   : "); Serial.println(whill.errorCode(lr));
}

void identify()
{
    pinMode(24, INPUT_PULLUP);
    pinMode(25, INPUT_PULLUP);
    pinMode(26, INPUT_PULLUP);
    pinMode(27, INPUT_PULLUP);

    HARDWARE_ID = (!digitalRead(24) << 3) | (!digitalRead(25) << 2) | (!digitalRead(26) << 1) | (!digitalRead(27) << 0);
    Serial.print("ID = "); Serial.println(HARDWARE_ID);
}

bool initEeprom(uint8_t hw_id)
{
    if (eepmap.checkHardwareID(hw_id))
    {
        Serial.println("EEPROM HW ID Check OK");
        return true;
    }
    else
    {
        Serial.print("EEPROM HW ID is different!! Writing Current ID = ");
        Serial.println(hw_id);
        eepmap.hardwareID(hw_id);
        b_need_xbee_init = true;
    }

    return false;
}

void setupXBee(uint8_t hw_id)
{
    xb_settings[0].channel = 0x0C;
    xb_settings[0].pan_id = 0x01;
    xb_settings[0].dst_addr_h = 0x00000000;
    xb_settings[0].dst_addr_l = 0x0000FFFF;
    xb_settings[0].packetization_timeout = 10;

    xb_settings[1].channel = 0x0D;
    xb_settings[1].pan_id = 0x02;
    xb_settings[1].dst_addr_h = 0x00000000;
    xb_settings[1].dst_addr_l = 0x0000FFFF;
    xb_settings[1].packetization_timeout = 10;

    xb_settings[2].channel = 0x12;
    xb_settings[2].pan_id = 0x03;
    xb_settings[2].dst_addr_h = 0x00000000;
    xb_settings[2].dst_addr_l = 0x0000FFFF;
    xb_settings[2].packetization_timeout = 10;

    xb_settings[3].channel = 0x0F;
    xb_settings[3].pan_id = 0x04;
    xb_settings[3].dst_addr_h = 0x00000000;
    xb_settings[3].dst_addr_l = 0x0000FFFF;
    xb_settings[3].packetization_timeout = 10;

    xb_settings[4].channel = 0x10;
    xb_settings[4].pan_id = 0x05;
    xb_settings[4].dst_addr_h = 0x00000000;
    xb_settings[4].dst_addr_l = 0x0000FFFF;
    xb_settings[4].packetization_timeout = 10;

    xb_settings[5].channel = 0x11;
    xb_settings[5].pan_id = 0x06;
    xb_settings[5].dst_addr_h = 0x00000000;
    xb_settings[5].dst_addr_l = 0x0000FFFF;
    xb_settings[5].packetization_timeout = 10;

    uint8_t index = 0xFF;
    if      (hw_id < 1) index = 5;
    else if (hw_id > 6) index = 5;
    else                index = hw_id - 1;

    xbeecmd.channel(xb_settings[index].channel);
    xbeecmd.panID(xb_settings[index].pan_id);
    xbeecmd.dstAddrH(xb_settings[index].dst_addr_h);
    xbeecmd.dstAddrL(xb_settings[index].dst_addr_l);
    xbeecmd.baudrate(0x5);
    xbeecmd.packtizationTimeout(xb_settings[index].packetization_timeout);

    xbeecmd.save();
    xbeecmd.apply();
    xbeecmd.reset();
    // xbeecmd.exit();
    XBEE_SERIAL.end();

    delay(1000);

    XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
    xbeecmd.enter();
}

void initXBee(uint8_t hw_id)
{
    XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
    xbeecmd.attach(XBEE_SERIAL);

    if (!b_need_xbee_init)
    {
        return; // no communication check

        // // check XBee Connection
        // if (xbeecmd.enter())
        // {
        //     if (xbeecmd.ack())
        //     {
        //         Serial.println("XBee Communication Check OK");
        //         xbeecmd.exit();
        //         return;
        //     }
        //     else
        //     {
        //         Serial.println("XBee Communication ERROR !! (NO ACK)");
        //         b_need_xbee_init = true;
        //     }
        // }
        // else
        // {
        //     Serial.println("XBee Communication ERROR !! (CANNOT ENTER)");
        //     b_need_xbee_init = true;
        // }
    }
    else
    {
        Serial.println("ID has been changed!! Re-Init XBee...");
        b_need_xbee_init = true;
    }

    // XBee initialize process

    XBEE_SERIAL.end();
    XBEE_SERIAL.begin(9600);

    if (!xbeecmd.enter())
    {
        Serial.println("XBee baudrate != 9600. Retry in default baudrate...");
        XBEE_SERIAL.end();
        XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
        if (!xbeecmd.enter())
        {
            // while(1)
            {
                Serial.println("XBee Communication Error!!! Check Connection w/ XBee...");
                static bool led = false;
                digitalWrite(13, led);
                led = !led;
                delay(1000);
            }
        }
    }
    else
    {
        Serial.println("XBee baudrate == 9600. Initilize XBee");
        b_need_xbee_init = true;
    }

#ifdef AUTO_CONFIGURE_XBEE
    if (b_need_xbee_init) setupXBee(hw_id);
#endif

    xbeecmd.firmwareVer();
    xbeecmd.hardwareVer();

    Serial.println("XBee Ready");

    xbeecmd.exit();
}

void setVelocity()
{
    static float vel[3];

    unpacker.parse();

    if (unpacker.isStatusReportChanged())
    {
        uint32_t status_report_duration = unpacker.statusReportDurationMs();

        if (status_report_duration == 0xFFFFFFFF)
        {
            b_streaming = false;
        }
        else
        {
            b_streaming = true;
            streaming_interval = status_report_duration;
        }
    }

    while (unpacker.available())
    {
        if (unpacker.id() == HARDWARE_ID)
        {
            vel[0] = unpacker.velocityX();
            vel[1] = unpacker.velocityY();
            vel[2] = unpacker.velocityW();
            stopwatch.start();
        }
        unpacker.pop();
    }

    fps.update();
    if (stopwatch.isRunning())
    {
        if (stopwatch.ms() < xbee_cmd_timeout_ms)
        {
            if (b_motor_active)
            {
                if (fps.isNext()) whill.move(vel[0], vel[1], vel[2]);
            }
            else
            {
                vel[0] = vel[1] = vel[2] = 0.f;
                Serial.println("Emergency Switch Pressed!! Motor Disabled...");
            }
        }
        else
        {
            vel[0] = vel[1] = vel[2] = 0.f;
            stopwatch.stop();
            Serial.println("XBee Communication Timeout...");
        }
    }
}

void sendStatus()
{
    if (b_streaming)
    {
        Serial.println("streaming enabled");
        if (b_motor_active)
            packer.pack(HARDWARE_ID, whill.battery(WhillOmni::LR::L), whill.battery(WhillOmni::LR::R), whill.errorCode(WhillOmni::LR::L), whill.errorCode(WhillOmni::LR::R));
        else
            packer.pack(HARDWARE_ID, whill.battery(WhillOmni::LR::L), whill.battery(WhillOmni::LR::R), 252, 252);

        XBEE_SERIAL.write(packer.data(), packer.size());
    }
    else
    {
        Serial.println("streaming disabled");
    }
}

void waitReply(uint16_t wait_ms = 0)
{
    delay(wait_ms);

    whill.parse();

    printSpeedProfile(WhillOmni::LR::L);
    printSpeedProfile(WhillOmni::LR::R);
    printSensorInfo(WhillOmni::LR::L);
    printSensorInfo(WhillOmni::LR::R);
}


template <typename WireType>
void i2c_Scan(WireType& wire)
{
    Serial.println ();
    Serial.println ("I2C scanner. Scanning ...");
    byte count = 0;

    wire.begin();
    for (byte i = 8; i < 120; i++)
    {
        wire.beginTransmission (i);
        if (wire.endTransmission () == 0)
        {
            Serial.print ("Found address: ");
            Serial.print (i, DEC);
            Serial.print (" (0x");
            Serial.print (i, HEX);
            Serial.println (")");
            count++;
            delay (1);
        }
    }
    Serial.println ("Done.");
    Serial.print ("Found ");
    Serial.print (count, DEC);
    Serial.println (" device(s).");
    Serial.println ("***********");
    Serial.println (" ");
}


#endif // MAIN_HEADER_H
