﻿/*
MIT License

Copyright(c) 2019 Mitchel Thompson
www.angryarugula.com

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#if !UNITY_WEBGL
using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System.IO;
using System;

namespace Arugula.Input
{
    public class XBee
    {
        const byte StartByte = 0x7E;
        const byte Escape = 0x7D;
        const byte XOn = 0x11;
        const byte XOff = 0x13;

        public enum APIMode
        {
            //None = 0,   //why?
            API = 1,
            APIEscaped = 2
        }

        public enum FrameType
        {
            ModemStatus = 0x8A,
            ATCommand = 0x08,
            ATCommandQueue = 0x09,  //basically never use this
            ATCommandResponse = 0x88,
            RemoteATCommandRequest = 0x17,
            RemoteATCommandResponse = 0x97,
            TXRequest64 = 0x00,
            TXRequest16 = 0x01,
            TXStatus = 0x89,
            RX64 = 0x80,
            RX16 = 0x81,
            RX64IO = 0x82,
            RX16IO = 0x83
        }

        public static class ATCommands
        {
            public const string Write = "WR";
            public const string RestoreDefaults = "RE";
            public const string SoftwareReset = "FR";
            public const string Channel = "CH";
            public const string PanID = "ID";
            public const string DestinationAddressHigh = "DH";
            public const string DestinationAddressLow = "DL";
            public const string SourceAddress16 = "MY";
            public const string SerialNumberHigh = "SH";
            public const string SerialNumberLow = "SL";
            public const string Retries = "RR";
            public const string RandomDelaySlots = "RN";
            public const string MacMode = "MM";
            public const string NodeIdentifier = "NI";
            public const string NodeDiscovery = "ND";
            public const string NodeDiscoveryTime = "NT";
            public const string NodeDiscoveryOptions = "NO";
            public const string DestinationNode = "DN";
        }

        public class Address
        {
            public Address()
            {

            }

            public Address(ref byte[] arr, int offset, int length)
            {
                if (length == 2)
                {
                    dynamicAddress = new byte[2];
                    Array.Copy(arr, offset, dynamicAddress, 0, length);
                }
                else if (length == 8)
                {
                    serialNumber = new byte[8];
                    Array.Copy(arr, offset, serialNumber, 0, length);
                }
            }

            public Address(params byte[] arr)
            {
                if (arr.Length == 2)
                    dynamicAddress = (byte[])arr.Clone();
                else if (arr.Length == 8)
                    serialNumber = (byte[])arr.Clone();
            }

            public byte[] dynamicAddress;
            public string dynamicAddressString { get { if (dynamicAddress == null) return null; string str = ""; foreach (var b in dynamicAddress) str += b.ToString("x2"); return str; } }
            public byte[] serialNumber;
            public string serialNumberString { get { if (serialNumber == null) return null; string str = ""; foreach (var b in serialNumber) str += b.ToString("x2"); return str; } }
            public string nodeIdentifier;

            public void SetDynamicAddress(ref byte[] arr, int offset, int length)
            {
                dynamicAddress = new byte[2];
                Array.Copy(arr, offset, dynamicAddress, 0, length);
            }

            public void SetSerialNumber(ref byte[] arr, int offset, int length)
            {
                serialNumber = new byte[8];
                Array.Copy(arr, offset, serialNumber, 0, 8);
            }

            public void SetNodeIdentifier(ref byte[] arr, int offset)
            {
                int length = 0;
                for (int i = offset; i < arr.Length; i++)
                {
                    if (arr[i] == 0x00)
                    {
                        length = i - offset;
                        break;
                    }
                }

                nodeIdentifier = System.Text.Encoding.ASCII.GetString(arr, offset, length);
            }



            public override string ToString()
            {
                System.Text.StringBuilder builder = new System.Text.StringBuilder();

                builder.Append("[Address]");

                if (dynamicAddress != null)
                {
                    builder.Append(" Dynamic: ");
                    foreach (var b in dynamicAddress)
                        builder.Append(b.ToString("x2"));
                }


                if (serialNumber != null)
                {
                    builder.Append(" Serial: ");
                    foreach (var b in serialNumber)
                        builder.Append(b.ToString("x2"));
                }

                if (nodeIdentifier != null)
                {
                    builder.Append(" NodeIdentifier: ");
                    builder.Append(nodeIdentifier);
                }

                return builder.ToString();
            }

            public override bool Equals(object obj)
            {
                if (obj.GetType() != typeof(Address))
                    return false;

                Address a = this;
                Address b = (Address)obj;

                bool matched = false;

                if (a.dynamicAddress != null && b.dynamicAddress != null)
                {
                    matched = true;
                    for (int i = 0; i < 2; i++)
                    {
                        if (a.dynamicAddress[i] != b.dynamicAddress[i])
                            return false;
                    }
                }

                if (a.serialNumber != null && b.serialNumber != null)
                {
                    matched = true;
                    for (int i = 0; i < 8; i++)
                    {
                        if (a.serialNumber[i] != b.serialNumber[i])
                            return false;
                    }
                }

                if (a.nodeIdentifier != null && b.nodeIdentifier != null)
                {
                    matched = true;
                    if (a.nodeIdentifier != b.nodeIdentifier)
                        return false;
                }

                return matched;
            }
        }

        public class RXPacket
        {

            public RXPacket(ref byte[] arr)
            {
                bool is64bit = arr[0] == (byte)FrameType.RX64;
                int offset = 1;
                Source = new Address(ref arr, offset, is64bit ? 8 : 2);
                offset += is64bit ? 8 : 2;
                RSSI = arr[offset++];
                options = arr[offset++];
                Data = new byte[arr.Length - offset];
                Array.Copy(arr, offset, Data, 0, Data.Length);
            }

            public enum OptionFlags
            {
                Broadcast = 0x02,
                PANBroadcast = 0x04
            }

            public Address Source;
            public byte RSSI;
            byte options;
            public byte[] Data;

            public bool IsBroadcast { get { return (options & (byte)OptionFlags.Broadcast) > 0; } }
            public bool IsPANBroadcast { get { return (options & (byte)OptionFlags.PANBroadcast) > 0; } }

            public string DataString
            {
                get
                {
                    return System.Text.Encoding.ASCII.GetString(Data);
                }
            }

            public override string ToString()
            {
                string dataStr = "";
                foreach (var b in Data)
                    dataStr += b.ToString("x2") + " ";

                return string.Format("[RXPacket] Source: {0} RSSI: {1}\r\n{2}", Source, RSSI.ToString("x2"), dataStr);
            }
        }

        public class TXStatus
        {
            public TXStatus(ref byte[] arr)
            {
                FrameID = arr[1];
                Status = (StatusFlags)arr[2];
            }

            public enum StatusFlags
            {
                Success,
                NoACK,
                CCAFailure,
                Purged
            }

            public byte FrameID;
            public StatusFlags Status;

            public override string ToString()
            {
                return string.Format("[TXStatus] FrameID: {0}  Status: {1}", FrameID, Status);
            }
        }

        public class ATCommandReponse
        {

            public ATCommandReponse(ref byte[] arr)
            {
                FrameID = arr[1];
                Command = "";
                Command += (char)arr[2];
                Command += (char)arr[3];
                Status = (StatusFlags)arr[4];
                Data = new byte[arr.Length - 5];
                Array.Copy(arr, 5, Data, 0, Data.Length);
            }

            public enum StatusFlags { OK, Error, InvalidCommand, InvalidParameter }

            public byte FrameID;
            public string Command;
            public StatusFlags Status;
            public bool HasData { get { return Data.Length > 0; } }
            public byte[] Data;
            public string DataString
            {
                get
                {
                    return System.Text.Encoding.ASCII.GetString(Data);
                }
            }

            public override string ToString()
            {
                string str = "";
                foreach (var b in Data)
                    str += b.ToString("x2") + " ";

                return string.Format("[ATCommandResponse] Command: {0} Status: {1}, Data: {2}", Command, Status, str);
            }
        }

        public class RemoteATCommandReponse
        {
            public RemoteATCommandReponse(ref byte[] arr)
            {
                FrameID = arr[1];
                Address = new Address();
                Address.SetSerialNumber(ref arr, 2, 8);
                Address.SetDynamicAddress(ref arr, 10, 2);
                Command = "";
                Command += (char)arr[12];
                Command += (char)arr[13];
                Status = (StatusFlags)arr[14];
                Data = new byte[arr.Length - 15];
                Array.Copy(arr, 15, Data, 0, Data.Length);
            }

            public enum StatusFlags { OK, Error, InvalidCommand, InvalidParameter, RemoteTransmissionFailed }

            public byte FrameID;
            public Address Address;
            public string Command;
            public StatusFlags Status;
            public byte[] Data;
            public string DataString
            {
                get
                {
                    return System.Text.Encoding.ASCII.GetString(Data);
                }
            }

            public override string ToString()
            {
                string str = "";
                foreach (var b in Data)
                    str += b.ToString("x2") + " ";

                return string.Format("[RemoteATCommandResponse] Address: {0}, Command: {1} Status: {2}, Data: {3}", Address, Command, Status, str);
            }
        }


        class FrameReader
        {
            static byte CalculateChecksum(ref byte[] arr, int length)
            {
                byte checksum = 0;
                for (int i = 0; i < length; i++)
                {
                    checksum += arr[i];
                }

                return (byte)(0xFF - checksum);
            }

            public event Action<byte[]> OnFrameReceived = delegate { };
            public event Action<ushort, byte[], byte> OnBadFrameReceived = delegate { };

            public enum State { Waiting, LengthMSB, LengthLSB, Data, Checksum }

            //TODO: name this better
            public FrameReader(bool escape)
            {
                this.escape = escape;
            }

            State state = State.Waiting;
            byte[] buffer = new byte[256];
            int index = 0;
            ushort length = 0;
            bool escapeNext = false;
            bool escape;

            public void Push(byte b)
            {

                if (escape && (index > 0 && b == StartByte))
                {
                    //bad packet!  received start byte in the middle of parsing.  AP=2
                    Clear();
                }

                if (index > 0 && (escape && b == Escape))
                {
                    escapeNext = true;
                    return;
                }

                if (escapeNext)
                {
                    b = (byte)(0x20 ^ b);
                    escapeNext = false;
                }


                switch (state)
                {
                    case State.Waiting:
                        if (b == 0x7E)
                            state++;
                        break;
                    case State.LengthMSB:
                        length += (ushort)(b << 8);
                        state++;
                        break;
                    case State.LengthLSB:
                        length += b;

                        if (length > buffer.Length)
                        {
                            //oh crap
                            Clear();
                            break;
                        }

                        state++;
                        break;
                    case State.Data:
                        buffer[index] = b;
                        index++;
                        if (index == length)
                            state++;
                        break;
                    case State.Checksum:
                        byte checksum = b;

                        if (checksum != CalculateChecksum(ref buffer, length))
                            OnBadFrameReceived(length, buffer, checksum);
                        else
                        {
                            byte[] data = new byte[length];
                            Array.Copy(buffer, 0, data, 0, length);
                            OnFrameReceived(data);
                        }

                        Clear();
                        break;
                }
            }

            void Clear()
            {
                length = 0;
                index = 0;
                state = State.Waiting;
            }
        }


        public event Action<RXPacket> OnRXPacketReceived = delegate { };
        public event Action<TXStatus> OnTXStatusReceived = delegate { };
        public event Action<ATCommandReponse> OnATCommandResponseReceived = delegate { };
        public event Action<RemoteATCommandReponse> OnRemoteATCommandResponseReceived = delegate { };
        public event Action<byte[]> OnUnhandledFrameReceived = delegate { };
        public event Action<ushort, byte[], byte> OnBadFrameReceived = delegate { };
        public event Action<Address> OnNodeDiscovered = delegate { };
        public event Action OnNodeDiscoveryFinished = delegate { };

        SerialPort port;
        byte[] buffer = new byte[4096];
        FrameReader frameReader;
        bool escape;

        public XBee()
        {
            escape = true;
        }

        //TODO rename this constructor to something smarter
        public XBee(APIMode apiMode)
        {
            escape = apiMode == APIMode.APIEscaped;
        }

        public void Initialize(string portName, int baudRate = 38400, Parity parity = Parity.None, int dataBits = 8, StopBits stopBits = StopBits.One)
        {
            port = new SerialPort(portName, baudRate, parity, dataBits, stopBits);
            port.ReadTimeout = 2;
            port.WriteTimeout = 2;
            port.Open();

            frameReader = new FrameReader(escape);

            frameReader.OnFrameReceived += ParseFrame;
            frameReader.OnBadFrameReceived += (length, data, checksum) => { OnBadFrameReceived(length, data, checksum); };

            OnATCommandResponseReceived += XBee_OnATCommandResponseReceived;
        }

        public void Shutdown()
        {
            if (port != null)
            {
                if (port.IsOpen)
                    port.Close();

                port = null;
            }
        }

        ~XBee()
        {
            Shutdown();
        }


        private void XBee_OnATCommandResponseReceived(ATCommandReponse response)
        {
            switch (response.Command)
            {
                case ATCommands.NodeDiscovery:
                    HandleNodeDiscoveryResponse(response);
                    break;
            }
        }

        void HandleNodeDiscoveryResponse(ATCommandReponse response)
        {
            if (response.Status != ATCommandReponse.StatusFlags.OK)
                return;

            if (!response.HasData)
            {
                OnNodeDiscoveryFinished();
                return;
            }

            Address address = new Address();
            address.SetDynamicAddress(ref response.Data, 0, 2);
            address.SetSerialNumber(ref response.Data, 2, 8);
            address.SetNodeIdentifier(ref response.Data, 11);

            OnNodeDiscovered(address);
        }

        private void ParseFrame(byte[] data)
        {
            FrameType frameType = (FrameType)data[0];
            switch (frameType)
            {
                case FrameType.RX16:
                case FrameType.RX64:
                    OnRXPacketReceived(new RXPacket(ref data));
                    break;
                case FrameType.TXStatus:
                    OnTXStatusReceived(new TXStatus(ref data));
                    break;
                case FrameType.ATCommandResponse:
                    OnATCommandResponseReceived(new ATCommandReponse(ref data));
                    break;
                case FrameType.RemoteATCommandResponse:
                    OnRemoteATCommandResponseReceived(new RemoteATCommandReponse(ref data));
                    break;
                default:
                    OnUnhandledFrameReceived(data);
                    break;
            }
        }

        public void Poll()
        {
            var byteCount = port.BytesToRead;
            if (byteCount > 0)
            {
                int bytesRead = port.Read(buffer, 0, byteCount);
                for (int i = 0; i < bytesRead; i++)
                {
                    frameReader.Push(buffer[i]);
                }
            }
        }

        public void Send(Address destination, params byte[] data)
        {
            byte[] payload = CreateTXRequestFrame(destination, globalFrameId, escape, data);
            IncrementGlobalFrameId();
            port.Write(payload, 0, payload.Length);
        }

        public void Command(string command, params byte[] data)
        {
            byte[] payload = CreateATCommandFrame(globalFrameId, command, escape, data);
            IncrementGlobalFrameId();
            port.Write(payload, 0, payload.Length);
        }

        public void RemoteCommand(Address address, string command, bool apply, params byte[] data)
        {
            byte[] payload = CreateRemoteATCommandFrame(address, globalFrameId, command, apply, escape, data);
            IncrementGlobalFrameId();
            port.Write(payload, 0, payload.Length);
        }



        static byte globalFrameId = 1;
        static void IncrementGlobalFrameId()
        {
            globalFrameId++;
            if (globalFrameId == 0x80)
                globalFrameId = 1;
        }

        enum TXRequestOptions { None = 0x00, DisableRetry = 0x01, UseBroadcastPANId = 0x04 }

        static byte[] CreateTXRequestFrame(Address destination, byte frameId, bool escape, params byte[] data)
        {
            return CreateTXRequestFrame(destination, frameId, TXRequestOptions.None, escape, data);
        }

        static byte[] CreateTXRequestFrame(Address destination, byte frameId, TXRequestOptions options, bool escape, params byte[] data)
        {
            if (data == null || data.Length > 100)
                return null;

            MemoryStream stream = new MemoryStream();
            stream.WriteByte(destination.serialNumber != null ? (byte)FrameType.TXRequest64 : (byte)FrameType.TXRequest16);
            stream.WriteByte(frameId);
            if (destination.serialNumber != null)
                stream.Write(destination.serialNumber, 0, destination.serialNumber.Length);
            else if (destination.dynamicAddress != null)
                stream.Write(destination.dynamicAddress, 0, destination.dynamicAddress.Length);
            else
                return null;

            stream.WriteByte((byte)options);

            stream.Write(data, 0, data.Length);

            return CreateFrame(stream.ToArray(), escape);
        }

        static byte[] CreateATCommandFrame(byte frameId, string command, bool escape, params byte[] data)
        {
            if (command.Length != 2)
                return null;

            MemoryStream stream = new MemoryStream();
            stream.WriteByte((byte)FrameType.ATCommand);
            stream.WriteByte(frameId);

            stream.WriteByte((byte)command[0]);
            stream.WriteByte((byte)command[1]);

            if (data != null && data.Length > 0)
                stream.Write(data, 0, data.Length);

            return CreateFrame(stream.ToArray(), escape);
        }

        static byte[] CreateRemoteATCommandFrame(Address address, byte frameId, string command, bool applyChanges, bool escape, params byte[] data)
        {
            if (command.Length != 2)
                return null;

            MemoryStream stream = new MemoryStream();
            stream.WriteByte((byte)FrameType.RemoteATCommandRequest);
            stream.WriteByte(frameId);

            if (address.serialNumber != null)
                stream.Write(address.serialNumber, 0, 8);
            else
                stream.Write(new byte[8], 0, 8);

            if (address.dynamicAddress != null)
                stream.Write(address.dynamicAddress, 0, 2);
            else
                stream.Write(new byte[2], 0, 2);


            stream.WriteByte(applyChanges ? (byte)0x02 : (byte)0x00);


            stream.WriteByte((byte)command[0]);
            stream.WriteByte((byte)command[1]);

            if (data != null && data.Length > 0)
                stream.Write(data, 0, data.Length);

            return CreateFrame(stream.ToArray(), escape);
        }

        static void Write(Stream stream, byte b, bool escaped = true)
        {
            if (escaped && (b == StartByte || b == Escape || b == XOn || b == XOff))
            {
                stream.WriteByte(Escape);
                stream.WriteByte((byte)(b ^ 0x20));
            }
            else
            {
                stream.WriteByte(b);
            }
        }
        static byte[] CreateFrame(byte[] payload, bool escape = true)
        {

            MemoryStream stream = new MemoryStream();

            Write(stream, StartByte, false);
            Write(stream, (byte)(payload.Length >> 8));
            Write(stream, (byte)(payload.Length & 0xFF));

            byte checksum = 0;

            foreach (var b in payload)
            {
                Write(stream, b, escape);
                checksum += b;
            }

            checksum = (byte)(0xff - checksum);

            Write(stream, checksum, escape);

            return stream.ToArray();
        }
    }
}
#endif