#pragma once
#ifndef XBEEATCMDS_H
#define XBEEATCMDS_H

namespace XBeeATCmdsUtil
{
    template <size_t size>
    struct same_size_int;

    template <size_t size>
    using same_size_int_t = typename same_size_int<size>::type;

    template <> struct same_size_int<1> { using type = int8_t; };
    template <> struct same_size_int<2> { using type = int16_t; };
    template <> struct same_size_int<4> { using type = int32_t; };
    template <> struct same_size_int<8> { using type = int64_t; };

    template <typename T, typename std::enable_if<std::is_floating_point<T>::value>::type* = nullptr>
    struct IntFloatUnion_impl {
        using type = union {
            same_size_int_t<sizeof(T)> x;
            T f;
        };
    };

    template <typename T>
    using IntFloatUnion = typename IntFloatUnion_impl<T>::type;

    template <typename T, size_t length = sizeof(T) * 2>
    auto toHex(T value)
    -> typename std::enable_if<std::is_integral<T>::value, String>::type
    {
        String format = "%0" + String(length) + "X";
        char hex[length + 1];

        sprintf(hex, format.c_str(), value);
        return String(hex);
    }

    template <typename T, size_t length = sizeof(T) * 2>
    auto toHex(T value)
    -> typename std::enable_if<std::is_floating_point<T>::value, String>::type
    {
        IntFloatUnion<T> myUnion;
        myUnion.f = value;

        return toHex(myUnion.x);
    }
}

class XBeeATCmds
{
public:

    void attach(Stream& s)
    {
        serial = &s;
    }

    // ----- enter AT Command mode -----

    bool enter()
    {
        Serial.print("ENTER COMMAND MODE: ");
        serial->print("+++");
        delay(WAIT_COMMAND_MODE_MS);
        return dumpReply();
    }

    bool ack()
    {
        Serial.print("ACK: ");
        serial->println("AT");
        return dumpReply();
    }


    // ----- special commands -----

    bool save()
    {
        Serial.print("WRITE DATA TO MEMORY: ");
        serial->println("ATWR");
        return dumpReply();
    }

    bool restore()
    {
        Serial.print("RESTORE DATA TO FACTORY DEFAULTS: ");
        serial->println("ATRE");
        return dumpReply();
    }

    bool reset()
    {
        Serial.print("RESET: ");
        serial->println("ATFR");
        return dumpReply();
    }


    // ----- network setting commands -----

    bool channel(uint8_t ch = 0xFF)
    {
        Serial.print("CHANNEL: ");
        serial->print("ATCH");
        if (ch == 0xFF) serial->println();
        else serial->println(XBeeATCmdsUtil::toHex(ch));
        return dumpReply();
    }

    bool panID(uint16_t pan = 0xFFFF)
    {
        Serial.print("PAN ID: ");
        serial->print("ATID");
        serial->println(XBeeATCmdsUtil::toHex(pan));
        return dumpReply();
    }

    bool dstAddrH(uint32_t addr)
    {
        Serial.print("DESTINATION ADDR H: ");
        serial->print("ATDH");
        serial->println(XBeeATCmdsUtil::toHex(addr));
        return dumpReply();
    }

    bool dstAddrL(uint32_t addr)
    {
        Serial.print("DESTINATION ADDR L: ");
        serial->print("ATDL");
        serial->println(XBeeATCmdsUtil::toHex(addr));
        return dumpReply();
    }

    bool sourceAddr(uint16_t addr)
    {
        Serial.print("DESTINATION ADDR L: ");
        serial->print("ATMY");
        serial->println(XBeeATCmdsUtil::toHex(addr));
        return dumpReply();
    }

    bool retries(uint8_t times)
    {
        Serial.print("XBEE RETRIES: ");
        serial->print("ATRR");
        serial->println(XBeeATCmdsUtil::toHex(times));
        return dumpReply();
    }

    bool coordinator(bool b)
    {
        Serial.print("COORDINATOR ENABLE: ");
        serial->print("ATCE");
        serial->println(XBeeATCmdsUtil::toHex((uint8_t)b));
        return dumpReply();
    }

    // SH : Serial Number High
    // SL : Serial Number Low
    // RN : Random Delay Slots
    // MM : MAC Mode
    // NI : Node Identifier
    // ND : Node Discover
    // NT : Node Discover Time
    // NO : Node Discovery Options
    // DN : Destination Node
    // SC : Scan Channel
    // SD : Scan Duration
    // A1 : End Device Association
    // A2 : Coordinator Association
    // AI : Association Indication
    // DA : Force Disassociation
    // FP : Force Poll
    // AS : Active Scan
    // ED : Energy Scan
    // EE : AES Encryption Enable
    // KY : AES Encryption Key


    // ----- RF Interfacing Commands -----

    bool powerLevel(uint8_t lvl)
    {
        Serial.print("POWER LEVEL: ");
        serial->print("ATPL");
        serial->println(XBeeATCmdsUtil::toHex(lvl));
        return dumpReply();
    }

    // CA : CCA Threshold


    // ----- Sleep Commands -----

    // SM : Sleep Mode
    // SO : Sleep Options
    // ST : Time before Sleep
    // SP : Cyclic Sleep Period
    // DP : Disassociated Cyclic Sleep Period


    // ----- Serial Interfacing Commands -----

    bool baudrate(uint8_t baudrate)
    {
        Serial.print("BAUDRATE: ");
        String baudrate_str = XBeeATCmdsUtil::toHex(baudrate);
        Serial.print(baudrate_str);
        Serial.print(" ");
        serial->print("ATBD");
        serial->println(baudrate_str);
        return dumpReply();
    }

    bool packtizationTimeout(uint8_t num)
    {
        Serial.print("PACKETIZATION TIMEOUT: ");
        serial->print("ATRO");
        serial->println(XBeeATCmdsUtil::toHex(num));
        return dumpReply();
    }

    bool apiEnable(uint8_t en)
    {
        Serial.print("API ENABLE: ");
        serial->print("ATAP");
        serial->println(XBeeATCmdsUtil::toHex(en));
        return dumpReply();
    }

    bool parity(uint8_t parity)
    {
        Serial.print("PARITY: ");
        serial->print("ATNB");
        serial->println(XBeeATCmdsUtil::toHex(parity));
        return dumpReply();
    }

    // PR : Pull-up/down Resistor Enable


    // ----- I/O Settings Commands -----

    // D0 : DIO0 Configuration
    // D1 : DIO1 Configuration
    // D2 : DIO2 Configuration
    // D3 : DIO3 Configuration
    // D4 : DIO4 Configuration
    // D5 : DIO5 Configuration
    // D6 : DIO6 Configuration
    // D7 : DIO7 Configuration
    // D8 : DIO8 Configuration
    // IU : I/O Output Enable
    // IT : Samples before TX
    // IS : Force Sample
    // IO : Digital Output Level
    // IC : DIO Change Detect
    // IR : Sample Rate
    // IA : I/O Input Address
    // T0 : D0 Output Timeout
    // T1 : D1 Output Timeout
    // T2 : D2 Output Timeout
    // T3 : D3 Output Timeout
    // T4 : D4 Output Timeout
    // T5 : D5 Output Timeout
    // T6 : D6 Output Timeout
    // T7 : D7 Output Timeout
    // P0 : PWM0 Configuration
    // P1 : PWM1 Configuration
    // M0 : PWM0 Output Level
    // M1 : PWM1 Output Level
    // PT : PWM Output Timeout
    // RP : RSSI PWM Timer


    // ----- Diagnostic Commands -----

    bool firmwareVer() // Firmware Version
    {
        Serial.print("FIRMWARE VERSION: ");
        serial->println("ATVR");
        return dumpReply();
    }

    bool hardwareVer() // Hardware Version
    {
        Serial.print("HARDWARE VERSION: ");
        serial->println("ATHV");
        return dumpReply();
    }

    bool rssi() // Last Packet RSSI
    {
        Serial.print("LAST PACKET RSSI: ");
        serial->println("ATDV");
        return dumpReply();
    }

    // EC : CCA Failures
    // AC : ACK Failures
    // ED : Energy Scan


    // ----- Command Mode Options -----

    void exit()
    {
        Serial.print("EXIT AT COMMAND MODE: ");
        serial->println("ATCN");
    }

    bool apply()
    {
        Serial.print("APPLY CHANGES: ");
        serial->println("ATAC");
        return dumpReply();
    }

    // CT : Command Mode Timeout
    // GT : Guard Times
    // CC : Command Sequence Character


private:

    bool dumpReply()
    {
        String reply = serial->readStringUntil('\n');

        if (reply == "") // timeout
        {
            Serial.println("XBee NOT Responding. Timeout...");
            return false;
        }
        else
        {
            Serial.print(reply);
        }
        return true;
    }

    static const uint16_t WAIT_COMMAND_MODE_MS = 3000;
    Stream* serial;

};

#endif // XBEEATCMDS_H
