powerfun-unity/Assets/Scripts/Devices/Ant/AntConnector.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using ANT_Managed_Library;
using UnityEngine;

namespace Assets.Scripts.Devices.Ant
{
    public class AntConnector : IDisposable
    {
        private readonly byte[] ANTPLUS_NETWORK_KEY = new byte[]
            {
                //Insert the ANT+ network key here:
                0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45
                //Distribution of source code containing the ANT+ Network Key is prohibited. 
                //You may not add the ANT+ Network Key to this source code and republish it. 
                //The ANT+ Network Key is available to ANT+ Adopters. 
                //Please refer to http://thisisant.com to become an ANT+ Adopter and access the key.
            };
        private readonly static object _lock = new object();
        private static ANT_Device _antDevice = null;
        private ANT_Channel searchChannel;
        private readonly List<ANT_Channel> _channels = new List<ANT_Channel>();
        private int ChannelCount
        {
            get
            {
                //if (_antDevice == null) return 0;
                //return _antDevice.getNumChannels();
                return _channels.Count;
            }
        }
        private bool IsBackgroundScanning { get; set; }
        //int searchingDeviceIndex = -1;

        /// <summary>
        /// 可以搜索的设备
        /// </summary>
        private List<AbstractAntDevice> deviceList = new List<AbstractAntDevice>();

        public bool IsAvailable = false;
        /// <summary>
        /// 设备号,频道编号
        /// </summary>
        public readonly List<UserdChannel> usedChannels = new List<UserdChannel>();
        /// <summary>
        /// 搜索到的设备
        /// </summary>
        public readonly List<AbstractAntDevice> discoveredDevices = new List<AbstractAntDevice>();

        private Action<AbstractAntDevice> _action;
        private static AntConnector _antConnector;
        private Action<object> _log;

        /// <summary>
        /// 
        /// </summary>
        /// <param name="action">探索到新的设备</param>
        /// <returns></returns>
        public static AntConnector Instance(Action<AbstractAntDevice> action = null, 
            Action<object> log = null)
        {
            if (_antConnector == null)
            {
                _antConnector = new AntConnector();
                _antConnector._action = action;
                _antConnector._log = log;
            }

            return _antConnector;
        }        

        //private DeviceService deviceService = new DeviceService();
        private AntConnector()
        {
            if (_antDevice == null)
            {
                //ANT_Common.enableDebugLogs();
                //findUsableAntDevice();
            }
            //CheckStatus();

            deviceList.Add(new FitDevice(""));
            deviceList.Add(new PowerDevice(""));
            deviceList.Add(new CadenceDevice(""));
            deviceList.Add(new HeartRateDevice(""));
            deviceList.Add(new BikeSpdCadDevice(""));
            deviceList.Add(new SpeedDevice(""));

            var timer = new System.Timers.Timer(1000);
            timer.AutoReset = true;
            timer.Elapsed += Timer_Elapsed;
            timer.Enabled = true;
        }

        private void Timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            CheckStatus();
        }

        public void CheckStatus()
        {
            if (_antDevice != null)
                return;
            //discoveredDevices.Add(new VirtualPowerDevice());
            this.CreateAntDevice();
        }

        private void CreateAntDevice()
        {
            try
            {
                if ((int)ANT_Common.getNumDetectedUSBDevices() == 0)
                    return;
                _antDevice = new ANT_Device();
                if (_antDevice.getNumChannels() <= 4)
                {
                    _antDevice = null;
                    return;
                }
                if (!_antDevice.setNetworkKey(0, ANTPLUS_NETWORK_KEY, 500))
                    throw new ApplicationException("Failed to set network key");
                IsAvailable = true;
                //_antDevice.configureAdvancedBurstSplitting(true);

                _antDevice.deviceResponse += DeviceResponse;
                _antDevice.serialError += new ANT_Device.dSerialErrorHandler(antDevice_serialError);
                for (int i = 1; i < _antDevice.getNumChannels(); i++)
                {
                    var channel = _antDevice.getChannel(i);
                    _channels.Add(channel);
                    channel.channelResponse += new dChannelResponseHandler(DeviceResponse);
                }

                if (IsAvailable)
                {
                    usedChannels.Add(new UserdChannel
                    {
                        DeviceNumber = "scan",
                        DeviceTypeId = "",
                        Index = 0
                    });
                    StartNextSearch();
                }
            }
            catch (Exception ex)
            {
                //ANT_Device.shutdownDeviceInstance(ref _antDevice);    //Don't leave here with an invalid device ref
                //throw new Exception("Could not connect to valid USB2: " + ex.Message);   //forward the exception
                //Log.ErrorLog(ex);
                //Debug.LogError(ex);
                //throw ex;
                Log(ex.Message);
            }
            finally
            {
            }
        }

        private void Log(string str)
        {
            //System.IO.File.AppendAllText(@"D:\work\PowerFun\PowerFun-Unity\Build\log.txt", str+"\r\n");
            //Debug.LogError(str);
        }

        private void antDevice_serialError(ANT_Device sender, ANT_Device.serialErrorCode error, bool isCritical)
        {
            //throw new NotImplementedException();
            Log("出错了"+ error.ToString());
            if (error != ANT_Device.serialErrorCode.DeviceConnectionLost)
                return;

            if (_antDevice != null)
            {
                _antDevice.deviceResponse -= DeviceResponse;
                _antDevice.serialError -= antDevice_serialError;
            }
            _antDevice = null;
            IsAvailable = false;
            foreach (var item in _channels)
            {
                item.channelResponse -= DeviceResponse;
            }
            searchChannel.channelResponse -= antChannel_channelResponse_FeSearch;
            searchChannel = null;
            foreach (var item in discoveredDevices)
            {
                item.State = DeviceState.Disconnected;
            }

            discoveredDevices.Clear();
            usedChannels.Clear();
            IsBackgroundScanning = false;

        }

        void StartNextSearch()
        {
            //Console.WriteLine("startNextSearch");
            if (searchChannel != null)
            {
                //searchChannel.Dispose();
            }

            //Get new search channel if neccesary
            if (searchChannel == null)
            {
                //if (usedChannels.Count >= ChannelCount)
                //    return;     //no free channels

                //Find the first free channel and start the search
                //for (int i = 0; i < ChannelCount; ++i)
                //{
                //    if (!usedChannels.Values.Contains((byte)i))
                //    {
                //        searchChannel = _antDevice.getChannel(i);
                //        searchChannel.channelResponse += new dChannelResponseHandler(antChannel_channelResponse_FeSearch);
                //        break;
                //    }
                //}
                searchChannel = _antDevice.getChannel(0); // _antDevice.getChannel(usedChannels["scan"]);
                searchChannel.channelResponse += new dChannelResponseHandler(antChannel_channelResponse_FeSearch);
            }
            IsBackgroundScanning = true;
            ReSearch();
        }

        void DeviceResponse(ANT_Response response)
        {
            //Debug.Log(response.responseID);
            var channelIndex = response.messageContents[0];

            var antMessageId = (ANT_ReferenceLibrary.ANTMessageID)response.responseID;
            if (antMessageId == ANT_Managed_Library.ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
            {
                //Trace.WriteLine(string.Join(",", response.messageContents));
                var antEventId = (ANT_ReferenceLibrary.ANTEventID)response.messageContents[2];
                //Trace.WriteLine(antEventId.ToString() +",  " + string.Join(",",response.messageContents));
                if (antEventId == ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_CLOSED_0x07)
                {

                }
                //设备断开连接
                else if (antEventId == ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08)
                {
                    _log?.Invoke(antEventId.ToString() + ",  " + string.Join(",", response.messageContents));

                    var cc1 = usedChannels.Where(d => d.Index == channelIndex).FirstOrDefault();
                    var dd1 = discoveredDevices.SingleOrDefault(d => d.searchProfile.deviceNumber.ToString() == cc1.DeviceNumber && d.searchProfile.deviceType.ToString() == cc1.DeviceTypeId);
                    if (dd1 != null)
                    {
                        dd1.Disconnect(false);
                        return;
                    }
                }
            }
            else
            {
                //Trace.WriteLine(antMessageId + ",    " + string.Join(",", response.messageContents));//EVENT_RX_FAIL_GO_TO_SEARCH_0x08                
            }

            //if (antMessageId == ANT_ReferenceLibrary.ANTMessageID.BURST_DATA_0x50)

            if (response.messageContents.Length == 3) return;
            var str = string.Join(",", response.messageContents);

            var cc = usedChannels.Where(d => d.Index == channelIndex).FirstOrDefault();
            var dd = discoveredDevices.SingleOrDefault(d => d.searchProfile.deviceNumber.ToString() == cc.DeviceNumber && d.searchProfile.deviceType.ToString() == cc.DeviceTypeId);
            if (dd != null)
            {
                if (dd.State == DeviceState.Connecting)
                {
                    dd.State = DeviceState.Connected;
                    //IsBackgroundScanning = true;
                    //ReSearch();
                    //StartNextSearch();
                    return;
                }
                dd.handleChannelResponse(response);
            }
        }

        public bool SendMessage(byte msgID, byte[] msgData)
        {
            return _antDevice.writeRawMessageToDevice(msgID, msgData);
        }

        public bool SendMessage(ANTMessage data)
        {
            return SendMessage(data.Id, data.Data);
        }

        void antChannel_channelResponse_FeSearch(ANT_Response response)
        {
            if (IsBackgroundScanning == false)
            {
                searchChannel.closeChannel();
                return;
            }

            if (response.messageContents.Length == 3) return;



            var message = response.messageContents;
            if (response.messageContents.Length >= 14)
            {
                message = response.messageContents.Skip(10).Take(4).ToArray();

                var deviceNumber = message[0] + (message[1] << 8);
                var deviceTypeId = message[2] & 127;
                var transmissionTypeId = message[3];

                //Console.WriteLine($"                                             {deviceNumber}, {deviceTypeId}");

                var device2 = discoveredDevices.FirstOrDefault(d => d.searchProfile.deviceNumber == deviceNumber && d.searchProfile.deviceType == deviceTypeId);
                if (device2 != null)
                {
                    //ReSearch();
                    _action?.Invoke(device2);
                    if (device2.State == DeviceState.Connected)
                    {
                        device2.handleChannelResponse(response);
                        return;
                    }
                    //else if (device2.State == DeviceState.Disconnected)
                    //{
                        //var deviceInfo = deviceService.Get(App.CurrentUser.Id, $"{ device2.searchProfile.deviceNumber }:{ device2.searchProfile.deviceType }");
                        //if (deviceInfo != null && deviceInfo.Paired)
                        //{
                        //    device2.Connect();
                        //}                        
                    //}
                    return;
                }
                var device1 = deviceList.SingleOrDefault(d => d.searchProfile.deviceType == deviceTypeId);
                if (device1 != null)
                {
                    var id = $"{ deviceNumber }:{ deviceTypeId}:{ transmissionTypeId }";
                    AbstractAntDevice device = null;
                    switch (device1.Sensor)
                    {
                        case SensorType.None:
                            break;
                        case SensorType.Cadence:
                            Debug.LogError("发现踏频设备"+id);
                            device = new CadenceDevice(id);
                            break;
                        case SensorType.HeartRate:
                            device = new HeartRateDevice(id);
                            break;
                        case SensorType.Power:
                            device = new PowerDevice(id);
                            break;
                        case SensorType.Speed:
                            device = new SpeedDevice(id);
                            break;
                        case SensorType.SpeedCadence:
                            device = new BikeSpdCadDevice(id);
                            break;
                        case SensorType.Trainer:
                            device = new FitDevice(id);
                            break;
                        case SensorType.VirtualPower:
                            break;
                        default:
                            break;
                    }
                    if (device != null)
                    {
                        //device.searchProfile.deviceNumber = (ushort)deviceNumber;
                        device.DeviceNumber = (ushort)deviceNumber;
                        discoveredDevices.Add(device);


                    }
                    //ReSearch();
                    return;
                }
            }
        }

        private void ReSearch()
        {
            if (IsBackgroundScanning == false) return;
            //Console.WriteLine($"research,{ DateTime.Now }");
            if (searchChannel != null)
            {
                //searchChannel.closeChannel(500);
                //searchChannel.unassignChannel();
                //searchChannel.openChannel();
                //return;
            }
            var channelIndex = (byte)0; //searchChannel.getChannelNum();
                                        //Handle setting the search timeout
            byte timeout = 4; //default 4*2.5=10 seconds for each device
                              //if (deviceList.Count - usedChannels.Count == 1)
            timeout = 255;  //search forever if we only have one device to find; If one of the other devices resets it will startNextSearch again so we won't get stuck

            searchChannel.assignChannelExt(ANT_ReferenceLibrary.ChannelType.ADV_TxRx_Only_or_RxAlwaysWildCard_0x40, 0, ANT_ReferenceLibrary.ChannelTypeExtended.ADV_AlwaysSearch_0x01);
            //SendMessage((int)ANT_ReferenceLibrary.ANTMessageID.ASSIGN_CHANNEL_0x42, new byte[] {
            //        channelIndex,
            //        (int)ANT_ReferenceLibrary.ChannelType.ADV_TxRx_Only_or_RxAlwaysWildCard_0x40,
            //        0
            //    });
            searchChannel.setChannelID(0, false, 0, 0);
            searchChannel.setChannelFreq(57);
            SendMessage(102, new byte[] { channelIndex, Convert.ToByte(true) });
            searchChannel.setLowPrioritySearchTimeout(timeout);
            searchChannel.setChannelSearchTimeout(0);
            searchChannel.openChannel(500);
        }

        public void ConnectDevice(AbstractAntDevice device)
        {
            if (usedChannels.Any(u => u.DeviceNumber == device.searchProfile.deviceNumber.ToString() &&
                u.DeviceTypeId == device.searchProfile.deviceType.ToString()))
                return;

            if (device.Sensor == SensorType.Trainer || device.Sensor == SensorType.Power)
            {
                if (discoveredDevices.Any(s => s.Sensor == device.Sensor && (s.State == DeviceState.Connected || s.State == DeviceState.Connecting)))
                {
                    return;
                }
            }
            if (device.Sensor == SensorType.Cadence)
            {
                if (discoveredDevices.Any(s => s.Sensor == device.Sensor && (s.State == DeviceState.Connected || s.State == DeviceState.Connecting)))
                {
                    return;
                }
            }
            if (device.Sensor == SensorType.HeartRate)
            {
                if (discoveredDevices.Any(s => s.Sensor == device.Sensor && (s.State == DeviceState.Connected || s.State == DeviceState.Connecting)))
                {
                    return;
                }
            }

            for (int i = 0; i < ChannelCount; i++)
            {
                if (!usedChannels.Any(u => u.Index == (byte)i))
                {
                    usedChannels.Add(new UserdChannel
                    {
                        DeviceNumber = device.searchProfile.deviceNumber.ToString(),
                        DeviceTypeId = device.searchProfile.deviceType.ToString(),
                        Index = (byte)i
                    });
                    break;
                }
            }

            //var deviceService = new DeviceService();
            //var info = deviceService.Get(App.CurrentUser.Id, device.searchProfile.deviceNumber.ToString() + ":" + device.searchProfile.deviceType);
            //if (info != null && info.Paired == false)
            //{
            //    info.Paired = true;
            //    deviceService.Update(info);
            //}

            #region 配对
            device.State = DeviceState.Connecting;

            //var r = searchChannel.closeChannel(500);
            //if (!r)
            //{
            //    throw new Exception("关闭搜索频道出错");
            //}

            //CloseChannel(searchChannel);
            //IsBackgroundScanning = false;

            var channelIndex = usedChannels.Single(c => c.DeviceNumber == device.searchProfile.deviceNumber.ToString() && c.DeviceTypeId == device.searchProfile.deviceType.ToString()).Index;

            var channel = _antDevice.getChannel(channelIndex);
            //channel.channelResponse += DeviceResponse;
            channel.assignChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00, 0);
            channel.setChannelID(device.searchProfile.deviceNumber, false, (byte)device.searchProfile.deviceType, device.searchProfile.transType);
            channel.setChannelPeriod(device.searchProfile.messagePeriod);
            channel.setChannelFreq(device.searchProfile.rfOffset);
            channel.openChannel();

            //if (!r1)
            //{
            //    throw new Exception($"打开配对频道出错{ channelIndex }");
            //}
            //Console.WriteLine($"                        连接设备{ device.searchProfile.deviceNumber }, 频道{ channelIndex },{ DateTime.Now }");
            

            //Task.Delay(2000).ContinueWith((t) =>
            //{
            //    IsBackgroundScanning = true;
            //    ReSearch();
            //});
            #endregion
        }

        public void DisconnectDevice(AbstractAntDevice device, bool save)
        {
            var deviceNumber = device.searchProfile.deviceNumber.ToString();
            var deviceType = device.searchProfile.deviceType.ToString();
            var item = usedChannels.SingleOrDefault(u => u.DeviceNumber == deviceNumber && u.DeviceTypeId == deviceType);
            if (item == null)
                return;
            device.State = DeviceState.Disconnecting;
            var channelIndex = item.Index;
            var channel = _antDevice.getChannel(channelIndex);
            CloseChannel(channel);
            usedChannels.Remove(item);
            device.State = DeviceState.Disconnected;

            if (save)
            {
                //var deviceService = new DeviceService();
                //var info = deviceService.Get(App.CurrentUser.Id, device.searchProfile.deviceNumber.ToString() + ":" + device.searchProfile.deviceType);
                //if (info != null && info.Paired)
                //{
                //    info.Paired = false;
                //    deviceService.Update(info);
                //}
            }
        }

        public void Dispose()
        {
            discoveredDevices.Clear();
            deviceList.Clear();

            _channels.Clear();

            if (_antDevice != null)
            {
                _antDevice.Dispose();
            }
        }

        public void CloseChannel(ANT_Channel channel)
        {
            channel.closeChannel(500);
            channel.unassignChannel(500);
        }
    }
}