HC_APTBS/Infrastructure/Kwp/KW1281Connection.cs

using HC_APTBS.Infrastructure.Kwp.Packets;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace HC_APTBS.Infrastructure.Kwp
{
    /// <summary>
    /// Manages a dialog with a VW controller using the KW1281 protocol.
    /// </summary>
    public interface IKW1281Connection
    {
        ControllerInfo ReadEcuInfo();

        void EndCommunication();

        List<ControllerIdent> ReadIdent();

        List<byte> ReadEeprom(ushort address, byte count);

        List<byte> ReadRomEeprom(ushort address, byte count);

        void CustomReset();

        void SendPacket(List<byte> packetBytes);

        List<Packet> SendCustom(List<byte> packetCustomBytes);

        /// <summary>
        /// Keep the dialog alive by sending an ACK and receiving a response.
        /// </summary>
        void KeepAlive();

        List<FaultCode> ReadFaultCodes();

        /// <summary>
        /// Clear all of the controllers fault codes.
        /// </summary>        
        /// <returns>True if successful.</returns>
        bool ClearFaultCodes();

        /// <summary>
        /// Set the controller's software coding and workshop code.
        /// </summary>
        /// <param name="controllerAddress"></param>
        /// <param name="softwareCoding"></param>
        /// <param name="workshopCode"></param>
        /// <returns>True if successful.</returns>
        bool SetSoftwareCoding(int controllerAddress, int softwareCoding, int workshopCode);

		IKwpCommon KwpCommon { get; }
    }

    public class KW1281Connection : IKW1281Connection
    {
        public ControllerInfo ReadEcuInfo()
        {
            var packets = ReceivePackets();
			return new ControllerInfo(packets.Where(b => !b.IsAckNak));
        }

        public ControllerInfo ReadEcuInfoCustom(Int32 pkt_count)
        {
            var packets = new List<Packet>();

            for(var i=0;i<pkt_count; i++)
            {
                var packet = ReceivePacket();
                packets.Add(packet); // TODO: Maybe don't add the packet if it's an Ack
                if (packet.Bytes.Count < 0x10) {
                    break;
                }
                if (packet is AckPacket || packet is NakPacket)
                {
                    break;
                }
                if (i == pkt_count - 1) break;
                SendAckPacket();
            }
            return new ControllerInfo(packets.Where(b => !b.IsAckNak));
        }

        public List<ControllerIdent> ReadIdent()
        {
            var idents = new List<ControllerIdent>();
            bool moreAvailable;
            do
            {
                System.Diagnostics.Debug.WriteLine("Sending ReadIdent packet");

				SendPacket(new List<byte> { (byte)PacketCommand.ReadIdent });

                var packets = ReceivePackets();
                var ident = new ControllerIdent(packets.Where(b => !b.IsAckNak));
                idents.Add(ident);

                moreAvailable = packets
					.OfType<AsciiDataPacket>()
                    .Any(b => b.MoreDataAvailable);
            } while (moreAvailable);

            return idents;
        }


        /// <summary>
        /// Reads a range of bytes from the EEPROM.
        /// </summary>
        /// <param name="address"></param>
        /// <param name="count"></param>
        /// <returns>The bytes or null if the bytes could not be read</returns>
        public List<byte> ReadEeprom(ushort address, byte count)
        {
            System.Diagnostics.Debug.WriteLine($"Sending ReadEeprom packet (Address: ${address:X4}, Count: ${count:X2})");
            SendPacket(new List<byte>
            {
                (byte)PacketCommand.ReadEeprom,
                count,
                (byte)(address >> 8),
                (byte)(address & 0xFF)
            });
            var packets = ReceivePackets();

            if (packets.Count == 1 && packets[0] is NakPacket)
            {
                // Permissions issue
                return null;
            }

            packets = packets.Where(b => !b.IsAckNak).ToList();
            if (packets.Count != 1)
            {
                throw new InvalidOperationException($"ReadEeprom returned {packets.Count} blocks instead of 1");
            }
            return packets[0].Body.ToList();
        }

        public List<byte> ReadRomEeprom(ushort address, byte count)
        {
            //System.Diagnostics.Debug.WriteLine($"Sending ReadRomEeprom packet (Address: ${address:X4}, Count: ${count:X2})");
            SendPacket(new List<byte>
            {
                (byte)PacketCommand.ReadRomEeprom,
                count,
                (byte)(address >> 8),
                (byte)(address & 0xFF)
            });
            var packets = ReceivePackets();

            if (packets.Count == 1 && packets[0] is NakPacket)
            {
                return new List<byte>();
            }

            packets = packets.Where(b => !b.IsAckNak).ToList();
            if (packets.Count != 1)
            {
                throw new InvalidOperationException($"ReadRomEeprom returned {packets.Count} blocks instead of 1");
            }
            return packets[0].Body.ToList();
        }


        public Dictionary<int, Packet> CustomReadSoftwareVersion()
        {
            var versionPackets = new Dictionary<int, Packet>();

            System.Diagnostics.Debug.WriteLine("Sending Custom \"Read Software Version\" packets");

			// The cluster can return 4 variations of software version, specified by the 2nd byte
			// of the packet:
			// 0x00 - Cluster software version
			// 0x01 - Unknown
			// 0x02 - Unknown
			// 0x03 - Unknown
			for (byte variation = 0x00; variation < 0x04; variation++)
            {
                var packets = SendCustom(new List<byte> { 0x84, variation });
                foreach (var packet in packets.Where(b => !b.IsAckNak))
                {
                    if (variation == 0x00 || variation == 0x03)
                    {
                        System.Diagnostics.Debug.WriteLine($"{variation:X2}: {DumpMixedContent(packet)}");
                    }
                    else
                    {
                        System.Diagnostics.Debug.WriteLine($"{variation:X2}: {DumpBinaryContent(packet)}");
                    }
					versionPackets[variation] = packet;
                }
            }

            return versionPackets;
        }

        private static string DumpMixedContent(Packet packet)
        {
            if (packet.IsNak)
            {
                return "NAK";
            }

            return DumpMixedContent(packet.Body);
        }

        /// <summary>
        /// Todo: Move to utility class
        /// </summary>
        public static string DumpMixedContent(IEnumerable<byte> content)
        {
            char mode = '?';
            var sb = new StringBuilder();
            foreach (var b in content)
            {
                if (b >= 32 && b <= 126)
                {
                    mode = 'A';

                    sb.Append((char)b);
                }
                else
                {
                    if (mode == 'A')
                    {
                        sb.Append(' ');
                    }
                    mode = 'X';

                    sb.Append($"${b:X2} ");
                }
            }
            return sb.ToString();
        }

        private static string DumpBinaryContent(Packet packet)
        {
            if (packet.IsNak)
            {
                return "NAK";
            }

            return DumpBytes(packet.Body);
        }

        private static string DumpBytes(IEnumerable<byte> bytes)
        {
            var sb = new StringBuilder();
            foreach (var b in bytes)
            {
                sb.Append($"${b:X2} ");
            }
            return sb.ToString();
        }

        public void CustomReset()
        {
            System.Diagnostics.Debug.WriteLine("Sending Custom Reset packet");
            SendCustom(new List<byte> { 0x82 });
        }

        public List<Packet> SendCustom(List<byte> packetCustomBytes)
        {
			SendPacket(packetCustomBytes);
            return ReceivePackets();
        }

        public void EndCommunication()
        {
            System.Diagnostics.Debug.WriteLine("Sending EndCommunication packet");
			SendPacket(new List<byte> { (byte)PacketCommand.End });
        }

        public void SendPacket(List<byte> packetBytes)
        {
            var packetLength = (byte)(packetBytes.Count + 2);

			packetBytes.Insert(0, _packetCounter.Value);
			_packetCounter++;

			packetBytes.Insert(0, packetLength);

            foreach (var b in packetBytes)
            {
                WriteByteAndReadAck(b);
                Thread.Sleep(5);
            }

            KwpCommon.WriteByte(0x03); // Packet end, does not get ACK'd
		}

        private List<Packet> ReceivePackets()
        {
            var packets = new List<Packet>();

            while (true)
            {
                var packet = ReceivePacket();
				packets.Add(packet); // TODO: Maybe don't add the packet if it's an Ack
                //SendAckPacket();

                if (packet is AckPacket || packet is NakPacket)
                {
                    break;
                }
				SendAckPacket();
            }

            return packets;
        }

        private void WriteByteAndReadAck(byte b)
        {
            KwpCommon.WriteByte(b);
            KwpCommon.ReadComplement(b);
        }

        private Packet ReceivePacket()
        {
            var packetBytes = new List<byte>();

            var packetLength = KwpCommon.ReadAndAckByte();
			packetBytes.Add(packetLength);

            var packetCounter = ReadPacketCounter();
			packetBytes.Add(packetCounter);

            var packetCommand = KwpCommon.ReadAndAckByte();
			packetBytes.Add(packetCommand);

            for (int i = 0; i < packetLength - 3; i++)
            {
                var b = KwpCommon.ReadAndAckByte();
				packetBytes.Add(b);
            }

            var packetEnd = KwpCommon.ReadByte();
			packetBytes.Add(packetEnd);
            if (packetEnd != 0x03)
            {
                throw new InvalidOperationException(
                    $"Received packet end ${packetEnd:X2} but expected $03");
            }


			switch (packetCommand) 
			{
				case (byte)PacketCommand.ACK:
					return new AckPacket(packetBytes);
					
				case (byte)PacketCommand.AsciiData:
					if (packetBytes[3] == 0x00) return new CodingWscPacket(packetBytes);
					return new AsciiDataPacket(packetBytes);

                case (byte)PacketCommand.ReadEepromResponse:
                    return new ReadEepromResponsePacket(packetBytes);

                case (byte)PacketCommand.ReadRomEepromResponse:
                    return new ReadRomEepromResponse(packetBytes);

				case (byte)PacketCommand.Custom:
					return new CustomPacket(packetBytes);
					break;

                case (byte)PacketCommand.NAK:
					return new NakPacket(packetBytes);
					
				case (byte)PacketCommand.FaultCodesResponse:
					return new FaultCodesPacket(packetBytes);

                case (byte)PacketCommand.WriteEepromResponse:
                    return new WriteEepromResponsePacket(packetBytes);

                default:
					return new UnknownPacket(packetBytes);
					
			}
			
		}

		private void SendAckPacket()
        {
            var packetBytes = new List<byte> { (byte)PacketCommand.ACK };
			SendPacket(packetBytes);
        }

        private byte ReadPacketCounter()
        {
            var packetCounter = KwpCommon.ReadAndAckByte();
            if (!_packetCounter.HasValue)
            {
				// First packet
				_packetCounter = packetCounter;
            }
            else if (packetCounter != _packetCounter)
            {
                throw new InvalidOperationException(
                    $"Received packet counter ${packetCounter:X2} but expected ${_packetCounter:X2}");
            }
			_packetCounter++;
            return packetCounter;
        }

        public void KeepAlive()
        {
			SendAckPacket();
            var packet = ReceivePacket();
			if (!(packet is AckPacket))
            {
                throw new InvalidOperationException(
                    $"Received 0x{packet.Title:X2} packet but expected ACK");
            }
        }


        public List<FaultCode> ReadFaultCodes()
        {
            System.Diagnostics.Debug.WriteLine($"Sending ReadFaultCodes packet");
			SendPacket(new List<byte>
            {
                (byte)PacketCommand.FaultCodesRead
            });

            var packets = ReceivePackets();
			packets = packets.Where(b => !b.IsAckNak).ToList();

            var faultCodes = new List<FaultCode>();
            var faultCodesData = new List<byte>();
            foreach (var packet in packets)
            {
                if (!(packet is FaultCodesPacket))
                {
                    System.Diagnostics.Debug.WriteLine($"Expected FaultCodesPacket but got {packet.GetType()}");
                    return null;
                }

                faultCodesData.AddRange(packet.Body);
            }

            IEnumerable<byte> data = faultCodesData;
            while (true)
            {
                var code = data.Take(3).ToArray();
                if (code.Length == 0)
                {
                    break;
                }

                var dtc = code[0];
                var status = code[1];

                var faultCode = new FaultCode(dtc, status);
                if (faultCode.Dtc != FaultCode.None.Dtc)
                {
                    faultCodes.Add(faultCode);
                }

                data = data.Skip(8);
            }

            return faultCodes;
        }

        public bool ClearFaultCodes()
        {
            System.Diagnostics.Debug.WriteLine($"Sending ClearFaultCodes packet");
			SendPacket(new List<byte>
            {
                (byte)PacketCommand.FaultCodesDelete
            });

            var packets = ReceivePackets();
            if (packets.Count == 1)
            {
                var packet = packets[0];
                if (packet is NakPacket)
                {
                    return false;
                }
                else if (packet is AckPacket)
                {
                    return true;
                }
                else
                {
                    throw new InvalidOperationException($"ClearFaultCodes returned {packet.GetType()} packet instead of ACK/NAK");
                }
            }
            else
            {
                throw new InvalidOperationException($"ClearFaultCodes returned {packets.Count} packets instead of 1");
            }
        }

        public bool SetSoftwareCoding(int controllerAddress, int softwareCoding, int workshopCode)
        {
            // Workshop codes > 65535 overflow into the low bit of the software coding
            var bytes = new List<byte>
            {
                (byte)PacketCommand.SoftwareCoding,
                (byte)((softwareCoding * 2) / 256),
                (byte)((softwareCoding * 2) % 256),
                (byte)((workshopCode & 65535) / 256),
                (byte)(workshopCode % 256)
            };

            if (workshopCode > 65535)
            {
                bytes[2]++;
            }

            System.Diagnostics.Debug.WriteLine($"Sending SoftwareCoding packet");
			SendPacket(bytes);

            var packets = ReceivePackets();
            if (packets.Count == 1 && packets[0] is NakPacket)
            {
                return false;
            }

            var controllerInfo = new ControllerInfo(packets.Where(b => !b.IsAckNak));
            return
                controllerInfo.SoftwareCoding == softwareCoding &&
                controllerInfo.WorkshopCode == workshopCode;
        }

        public IKwpCommon KwpCommon { get; }

        private byte? _packetCounter = null;

        public KW1281Connection(IKwpCommon kwpCommon)
        {
            KwpCommon = kwpCommon;
        }
    }

    public class KW1281KeepAlive : IDisposable
    {
        private readonly IKW1281Connection _kw1281Connection;
        private volatile bool _cancel = false;
        private Task _keepAliveTask = null;

        public KW1281KeepAlive(IKW1281Connection kw1281Connection)
        {
            _kw1281Connection = kw1281Connection;
        }

        public void Dispose()
        {
            Pause();
        }

        private void Pause()
        {
            _cancel = true;
            if (_keepAliveTask != null)
            {
                _keepAliveTask.Wait();
            }
        }

        private void Resume()
        {
            _keepAliveTask = Task.Run(KeepAlive);
        }

        private void KeepAlive()
        {
            _cancel = false;
            while (!_cancel)
            {
                _kw1281Connection.KeepAlive();
                Console.Write(".");
            }
        }
    }
}