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

namespace Aidlab.BLE
{
    public class BLEConnector : IBleConnector
    {
        private readonly object devicesLock = new object();
        private Dictionary<string, BLEApi.DeviceUpdate> devices;
        private string foundDevice;
        public Thread bleConnectorThread;
        private BLEStatus currentBLEStatus = BLEStatus.None;
	        private AidlabSDK aidlabSDK;
	        private string deviceNameToConnect;      
	        private bool isConnected = false;  
	        private string lastError = "";

	        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 string ConnectedDeviceId => foundDevice ?? "";

        public List<BLEApi.DeviceUpdate> GetKnownDevicesSnapshot()
        {
            lock (devicesLock)
            {
                return devices.Values.Select(BLEApi.DeviceUpdateCopy).ToList();
            }
        }

        #region MainMethods
	        public BLEConnector(AidlabSDK aidlabSDK, string deviceNameToConnect)
	        {
                      
            this.aidlabSDK = aidlabSDK;
            this.deviceNameToConnect = deviceNameToConnect;
            devices = new Dictionary<string, BLEApi.DeviceUpdate>();

	            bleConnectorThread = new Thread(StartDevicesScan) { IsBackground = true };
	            bleConnectorThread.Start();
	            currentBLEStatus = BLEStatus.ScanningDevices;

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

	            // Disconnect aborts the receive thread, 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:
                    Debug.Log("Something went wrong...");
                    break;
                case BLEStatus.ScanningDevices:
                    if (!bleConnectorThread.IsAlive && !string.IsNullOrEmpty(foundDevice))
                    {
                        currentBLEStatus = BLEStatus.TryingToConnect;
                        bleConnectorThread = new Thread(Connect) { IsBackground = true };
                        bleConnectorThread.Start();
                    }
                    break;
	                case BLEStatus.TryingToConnect:
	                    if (!bleConnectorThread.IsAlive && isConnected)
	                    {
	                        currentBLEStatus = BLEStatus.Connected;
	                        aidlabSDK.OnAidlabConnected();

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

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

        public byte[] AddByteToArray(byte[] bArray, byte newByte)
        {
            byte[] newArray = new byte[bArray.Length + 1];
            bArray.CopyTo(newArray, 0);
            newArray[bArray.Length] = newByte;
            return newArray;
        }

        // Called by the SDK, which reports the disconnection and replaces this connector, so its threads end.
        public void Disconnect()
        {
            isConnected = false;
            rxRunning = false;
            rxSignal.Set();
            writerRunning = false;
            writeSignal.Set();
            bleConnectorThread?.Abort();
            BLEApi.Quit();
        }
        #endregion MainMethods


        #region ScanningDevices
        private void StartDevicesScan()
        {
            Debug.Log("Start scanning devices");

            while (true)
            {
                BLEApi.StartDeviceScan();
                BLEApi.DeviceUpdate device = new BLEApi.DeviceUpdate();

                while (BLEApi.PollDevice(ref device, true) != BLEApi.ScanStatus.FINISHED)
                {
                    UpdateDevices(device);
                    foundDevice = CheckDevices();
                    if (!string.IsNullOrEmpty(foundDevice))
                    {
                        Debug.Log("Device found: " + foundDevice);

                        BLEApi.StopDeviceScan();
                        Thread.Sleep(256);
                        return;
                    }
                }

                BLEApi.StopDeviceScan();
                Debug.Log("Devices scanning complete");
                Thread.Sleep(500);
                Debug.Log("Start scanning devices again");
            }
        }


        private void UpdateDevices(BLEApi.DeviceUpdate device)
        {
            lock (devicesLock)
            {
                if (!devices.ContainsKey(device.id))
                {
                    var newDevice = BLEApi.DeviceUpdateCopy(device);
                    devices.Add(device.id, newDevice);
                }
                else
                {
                    var savedDevice = devices[device.id];
                    devices.Remove(device.id);

                    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.Add(device.id, newDevice);
                }
            }
        }

        private string CheckDevices()
        {
            lock (devicesLock)
            {
                foreach (var device in devices.Values)
                {
                    if (AidlabSDK.MatchesTargetName(device.name, deviceNameToConnect) && device.isConnectable)
                    {
                        return device.id;
                    }
                }
            }

            return null;
        }
        #endregion ScanningDevices


        #region Connecting
        private void Connect()
        {
            foreach (var characteristic in AidlabCharacteristicsUUID.Characteristics)
            {
                BLEApi.SubscribeCharacteristic(foundDevice,
                    characteristic.serviceUuid,
                    characteristic.characteristicUuid,
                    true);
                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(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(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()
        {

            while (rxRunning)
            {
                RawReceiveData();
            }
        }

	        private void RawReceiveData()
	        {

            BLEApi.BLEData BLEData;

            bool result = BLEApi.PollData(out BLEData, true);

	            if (!result)
	                return;

	            lock (rxQueue)
	            {
	                if (rxOverflowed)
	                    return;
	                if (rxQueue.Count < 8192)
	                {
	                    rxQueue.Enqueue(BLEData);
	                    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)
	                        continue;

	                    SendChunked(payload);
	                }
	            }
	        }

	        private void SendChunked(byte[] payload)
	        {
	            int offset = 0;
	            while (offset < payload.Length)
	            {
	                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);
	        }
	    }
	}
