﻿using System.Collections.Generic;
using System.Threading;
using UnityEngine;
using System;
using System.Linq;

namespace Aidlab.BLE
{
    public class BLEConnector : IBleConnector
    {
        private string foundDevice;
        public Thread bleConnectorThread;
        private BLEStatus currentBLEStatus = BLEStatus.None;
	        private AidlabSDK aidlabSDK;
	        private string deviceNameToConnect;
	        private readonly string deviceAddressToConnect;
	        private bool isConnected = false;  
	        private string lastError = "";
	        // The DLL keeps one error for every device, so it is logged once, not by each connector.
	        private static string loggedError = "";

	        // Disconnect releases the device at once, or, while Connect or a write still uses it, the last of them does
	        // when it ends, so that a call completing after Disconnect does not connect the device again.
	        private readonly object lifecycleLock = new object();
	        private volatile bool disposed;
	        private int deviceUsers;

	        private readonly Queue<BLEApi.BLEData> rxQueue = new Queue<BLEApi.BLEData>();
	        private readonly AutoResetEvent rxSignal = new AutoResetEvent(false);
	        private volatile bool rxRunning = true;
	        private bool rxOverflowed;

	        private readonly Queue<byte[]> writeQueue = new Queue<byte[]>();
	        private readonly AutoResetEvent writeSignal = new AutoResetEvent(false);
	        private Thread writerThread;
	        private volatile bool writerRunning = true;
	        private const int DefaultChunkSize = 20;
	        private const int MaxChunkSize = 512;

        public BLEStatus CurrentStatus => currentBLEStatus;
        public bool IsConnected => isConnected;
        public string LastError => lastError ?? "";

        public List<BLEApi.DeviceUpdate> GetKnownDevicesSnapshot()
        {
            return Scanner.Snapshot();
        }

        #region MainMethods
	        public BLEConnector(AidlabSDK aidlabSDK, string deviceNameToConnect, string deviceAddressToConnect)
	        {
                      
            this.aidlabSDK = aidlabSDK;
            this.deviceNameToConnect = deviceNameToConnect;
            this.deviceAddressToConnect = deviceAddressToConnect;

	            Scanner.Wait();
	            currentBLEStatus = BLEStatus.ScanningDevices;

	            writerThread = new Thread(WriterLoop);
	            writerThread.IsBackground = true;
	            writerThread.Start();

	            // The receive thread waits in PollData, so the core gets the data on a thread of its own.
	            var rxThread = new Thread(ProcessReceivedData);
	            rxThread.IsBackground = true;
	            rxThread.Start();
	        }

        public void ConnectionProcess()
        {
            switch (currentBLEStatus)
            {
                case BLEStatus.None:
                    break;
                case BLEStatus.ScanningDevices:
                    foundDevice = Scanner.TryHold(deviceNameToConnect, deviceAddressToConnect);
                    if (foundDevice != null)
                    {
                        Debug.Log("Device found: " + foundDevice);
                        Scanner.Done();
                        UseDevice();
                        currentBLEStatus = BLEStatus.TryingToConnect;
                        bleConnectorThread = new Thread(Connect) { IsBackground = true };
                        bleConnectorThread.Start();
                    }
                    break;
	                case BLEStatus.TryingToConnect:
	                    if (!bleConnectorThread.IsAlive && isConnected)
	                    {
	                        currentBLEStatus = BLEStatus.Connected;
	                        aidlabSDK.DeviceAddress = AddressOf(foundDevice);
	                        aidlabSDK.OnAidlabConnected();

	                        bleConnectorThread = new Thread(ReceiveData) { IsBackground = true };
	                        bleConnectorThread.Start();
	                    }
	                    break;
            }

            BLEApi.ErrorMessage res = new BLEApi.ErrorMessage();
            BLEApi.GetError(out res);
            lastError = res.msg;
            if (loggedError != res.msg)
            {
                loggedError = res.msg;
                Debug.Log("BleDll: " + res.msg);
            }
        }

        // Called by the SDK, which reports the disconnection and replaces this connector, so its threads end. The other
        // connectors keep their devices.
        public void Disconnect()
        {
            if (disposed)
                return;
            isConnected = false;
            rxRunning = false;
            rxSignal.Set();
            writerRunning = false;
            writeSignal.Set();
            if (currentBLEStatus == BLEStatus.ScanningDevices)
                Scanner.Done();
            lock (lifecycleLock)
            {
                disposed = true;
                if (deviceUsers == 0)
                    ReleaseDevice();
            }
        }

        // Whether Connect or a write may use the device; each true is paired with EndDeviceUse.
        private bool UseDevice()
        {
            lock (lifecycleLock)
            {
                if (disposed)
                    return false;
                deviceUsers++;
                return true;
            }
        }

        private void EndDeviceUse()
        {
            lock (lifecycleLock)
            {
                deviceUsers--;
                if (disposed && deviceUsers == 0)
                    ReleaseDevice();
            }
        }

        // Wakes the receive thread too: PollData returns at once once the device is disconnected.
        private void ReleaseDevice()
        {
            if (foundDevice == null)
                return;
            BLEApi.DisconnectDevice(foundDevice);
            AidlabSDK.ReleaseDevice(foundDevice);
        }

        // The Bluetooth address that ends a Windows device ID, BluetoothLE#BluetoothLE<adapter>-<device>.
        private static string AddressOf(string deviceId)
        {
            int dash = deviceId.LastIndexOf('-');
            return dash >= 0 ? deviceId.Substring(dash + 1) : deviceId;
        }
        #endregion MainMethods


        #region ScanningDevices
        // The DLL has one device watcher, so one scan serves every connector that waits for a device. It runs while any
        // does and keeps what it saw of each device since it started.
        private static class Scanner
        {
            private static readonly object sync = new object();
            private static readonly Dictionary<string, BLEApi.DeviceUpdate> devices =
                new Dictionary<string, BLEApi.DeviceUpdate>();
            private static int waiting;
            private static Thread thread;

            internal static void Wait()
            {
                lock (sync)
                {
                    waiting++;
                    if (thread != null)
                        return;
                    devices.Clear();
                    thread = new Thread(Run) { IsBackground = true };
                    thread.Start();
                }
            }

            internal static void Done()
            {
                lock (sync)
                {
                    waiting--;
                    if (waiting == 0)
                        BLEApi.StopDeviceScan();
                }
            }

            internal static List<BLEApi.DeviceUpdate> Snapshot()
            {
                lock (sync)
                    return devices.Values.Select(BLEApi.DeviceUpdateCopy).ToList();
            }

            // Holds and returns a connectable device that no connector holds, or null: the one with the target address,
            // or without an address one that matches the target name.
            internal static string TryHold(string deviceNameToConnect, string deviceAddressToConnect)
            {
                lock (sync)
                {
                    foreach (var device in devices.Values)
                    {
                        bool isTarget = string.IsNullOrEmpty(deviceAddressToConnect)
                            ? AidlabSDK.MatchesTargetName(device.name, deviceNameToConnect)
                            : string.Equals(AddressOf(device.id), deviceAddressToConnect, StringComparison.OrdinalIgnoreCase);
                        if (isTarget && device.isConnectable && AidlabSDK.TryHoldDevice(device.id))
                        {
                            return device.id;
                        }
                    }
                }
                return null;
            }

            private static void Run()
            {
                Debug.Log("Start scanning devices");

                while (true)
                {
                    // Under sync, so that Done stops a scan that has started or this one sees that nobody waits.
                    lock (sync)
                    {
                        if (waiting == 0)
                        {
                            thread = null;
                            return;
                        }
                        BLEApi.StartDeviceScan();
                    }

                    BLEApi.DeviceUpdate device = new BLEApi.DeviceUpdate();
                    while (BLEApi.PollDevice(ref device, true) != BLEApi.ScanStatus.FINISHED)
                        Update(device);

                    BLEApi.StopDeviceScan();
                    Thread.Sleep(500);
                }
            }

            private static void Update(BLEApi.DeviceUpdate device)
            {
                lock (sync)
                {
                    if (!devices.TryGetValue(device.id, out BLEApi.DeviceUpdate savedDevice))
                    {
                        devices.Add(device.id, BLEApi.DeviceUpdateCopy(device));
                        return;
                    }

                    var newDevice = BLEApi.DeviceUpdateCopy(savedDevice);
                    if (device.nameUpdated)
                    {
                        newDevice.name = device.name;
                        newDevice.nameUpdated = true;
                    }
                    if (device.isConnectableUpdated)
                    {
                        newDevice.isConnectable = device.isConnectable;
                        newDevice.isConnectableUpdated = true;
                    }
                    devices[device.id] = newDevice;
                }
            }
        }
        #endregion ScanningDevices


        #region Connecting
        private void Connect()
        {
            try
            {
                ConnectDevice();
            }
            finally
            {
                EndDeviceUse();
            }
        }

        private void ConnectDevice()
        {
            foreach (var characteristic in AidlabCharacteristicsUUID.Characteristics)
            {
                if (disposed)
                    return;
                // Without notifications the session cannot work, and the receive thread would find nothing to wait for.
                if (!BLEApi.SubscribeCharacteristic(foundDevice,
                    characteristic.serviceUuid,
                    characteristic.characteristicUuid,
                    true))
                {
                    aidlabSDK.OnTransportFailure($"BLEConnector: subscribing to {characteristic.characteristicUuid} failed", this);
                    return;
                }
                Thread.Sleep(128);
            }

            string fw = Read(AidlabCharacteristicsUUID.FirmwareUUID);
            string hw = Read(AidlabCharacteristicsUUID.HardwareUUID);

            aidlabSDK.firmwareRevisionStr = fw;
            aidlabSDK.hardwareRevisionStr = hw;

            // Firmware timestamps sessions with its clock, so set it like the other platforms: Unix time in seconds,
            // little-endian, written with response.
            uint now = (uint)DateTimeOffset.UtcNow.ToUnixTimeSeconds();
            BLEApi.SetWriteWithoutResponse(foundDevice, false);
            byte[] time = { (byte)now, (byte)(now >> 8), (byte)(now >> 16), (byte)(now >> 24) };
            if (!RawWriteInternal(AidlabCharacteristicsUUID.CurrentTimeUUID, time, 0, time.Length))
            {
                aidlabSDK.ReportError(new AidlabError(AidlabAPI.AidlabErrorCode.Transport, "Failed to set time"));
            }
            BLEApi.SetWriteWithoutResponse(foundDevice, ParseMajorFirmware(fw) >= 4);

            Debug.Log("Connection completed");
            Debug.Log("FW: " + fw);
            Debug.Log("HW: " + hw);
            
            isConnected = true;
        }
  
	        public void Write(byte[] payload)
	        {
	            if (payload == null || payload.Length == 0)
	                return;

	            lock (writeQueue)
	            {
	                writeQueue.Enqueue(payload);
	            }
	            writeSignal.Set();
	        }

	        public void RawWrite(byte[] payload) 
	        {
	            if (payload != null)
	                RawWriteInternal(AidlabCharacteristicsUUID.CmdUUID, payload, 0, payload.Length);
	        }

	        // Writes size bytes of payload from offset, at most MaxChunkSize.
	        private bool RawWriteInternal(FullCharacteristic characteristic, byte[] payload, int offset, int size)
	        {
	            if (size == 0)
	                return true;
	            if (size > MaxChunkSize)
	                return false;

	            BLEApi.BLEData data = new BLEApi.BLEData();
	            data.buf = new byte[MaxChunkSize];
	            data.size = (short)size;
	            data.deviceId = foundDevice;
	            data.serviceUuid = characteristic.serviceUuid;
	            data.characteristicUuid = characteristic.characteristicUuid;
	            Buffer.BlockCopy(payload, offset, data.buf, 0, size);

	            return BLEApi.SendData(in data, true);
	        }

	        private static int ParseMajorFirmware(string firmwareRevision)
	        {
	            if (string.IsNullOrEmpty(firmwareRevision))
	                return 0;

	            int nul = firmwareRevision.IndexOf('\0');
	            if (nul >= 0)
	                firmwareRevision = firmwareRevision.Substring(0, nul);

	            int dash = firmwareRevision.IndexOf('-');
	            if (dash >= 0)
	                firmwareRevision = firmwareRevision.Substring(0, dash);

	            firmwareRevision = firmwareRevision.Trim();
	            if (firmwareRevision.Length == 0)
	                return 0;

	            int dot = firmwareRevision.IndexOf('.');
	            string majorStr = dot >= 0 ? firmwareRevision.Substring(0, dot) : firmwareRevision;
	            if (int.TryParse(majorStr, out int major))
	                return major;
	            return 0;
	        }

        public String Read(FullCharacteristic characteristic)
        {

            BLEApi.ReadDataResult result;
            BLEApi.BLEData data = new BLEApi.BLEData();
            data.deviceId = foundDevice;
            data.serviceUuid = characteristic.serviceUuid;
            data.characteristicUuid = characteristic.characteristicUuid;

            BLEApi.ReadData(in data, out result);

            String value = "";

            if (result.result) {
                byte[] buf = result.buf.Reverse().SkipWhile(x => x == 0).Reverse().ToArray();
                value = System.Text.Encoding.UTF8.GetString(buf);
            }

            return String.Concat(value.Where(c => !Char.IsWhiteSpace(c)));
        }
        #endregion Connecting

        #region ReceiveData
        private void ReceiveData()
        {
            BLEApi.BLEData data;
            // PollData waits for data and ends without any once the device is gone.
            while (BLEApi.PollData(foundDevice, out data, true))
                Received(data);

            // Gone without Disconnect: Windows reported that the device disconnected.
            AidlabSDK.EnqueueMainThreadEvent(() =>
            {
                if (!disposed)
                    aidlabSDK.OnAidlabDisconnected();
            });
        }

	        private void Received(BLEApi.BLEData data)
	        {
	            lock (rxQueue)
	            {
	                if (rxOverflowed)
	                    return;
	                if (rxQueue.Count < 8192)
	                {
	                    rxQueue.Enqueue(data);
	                    rxSignal.Set();
	                    return;
	                }
	                // A dropped packet would corrupt the protocol framing, so the session ends instead.
	                rxOverflowed = true;
	                rxQueue.Clear();
	            }
	            aidlabSDK.OnTransportFailure("BLEConnector: RX queue full", this);
	        }
	        #endregion ReceiveData

	        // Off the main thread, so that acknowledgements do not wait for a Unity frame.
	        private void ProcessReceivedData()
	        {
	            while (rxRunning)
	            {
	                rxSignal.WaitOne();

	                // Stops with the connector, so that no data of this connection reaches the next one.
	                while (rxRunning)
	                {
	                    BLEApi.BLEData data;
	                    lock (rxQueue)
	                    {
	                        if (rxQueue.Count == 0)
	                            break;
	                        data = rxQueue.Dequeue();
	                    }

	                    aidlabSDK.OnAidlabDataReceived(data);
	                }
	            }
	        }

	        private void WriterLoop()
	        {
	            while (writerRunning)
	            {
	                writeSignal.WaitOne();

	                while (true)
	                {
	                    byte[] payload = null;
	                    lock (writeQueue)
	                    {
	                        if (writeQueue.Count > 0)
	                            payload = writeQueue.Dequeue();
	                    }

	                    if (payload == null)
	                        break;

	                    if (!isConnected || !UseDevice())
	                        continue;
	                    try
	                    {
	                        SendChunked(payload);
	                    }
	                    finally
	                    {
	                        EndDeviceUse();
	                    }
	                }
	            }
	        }

	        private void SendChunked(byte[] payload)
	        {
	            int offset = 0;
	            while (offset < payload.Length)
	            {
	                if (disposed)
	                    return;
	                int size = Math.Min(DefaultChunkSize, payload.Length - offset);
	                if (!RawWriteInternal(AidlabCharacteristicsUUID.CmdUUID, payload, offset, size))
	                {
	                    lock (writeQueue)
	                    {
	                        writeQueue.Clear();
	                    }
	                    writerRunning = false;
	                    aidlabSDK.OnTransportFailure($"BLEConnector: SendData failed (chunk {size}B)", this);
	                    return;
	                }
	                offset += size;
	            }
	            aidlabSDK.OnTransportFrameWritten(payload);
	        }
	    }
	}
