using System;
using System.Diagnostics;

namespace HC_APTBS.Infrastructure.Kwp
{
    public interface IKwpCommon
    {
        IInterface Interface { get; }

        int WakeUp(byte controllerAddress, bool evenParity = false);

        byte ReadByte();

        void WriteByte(byte b);

        byte ReadAndAckByte();

        void ReadComplement(byte b);
    }

    public class KwpCommon : IKwpCommon
    {
        public IInterface Interface => _interface;

        public int WakeUp(byte controllerAddress, bool evenParity = false)
        {
            // Disable garbage collection in this time-critical method
            bool noGc = GC.TryStartNoGCRegion(1024 * 1024 * 128); //antes 16

            BitBang5Baud(controllerAddress, evenParity);

            if (noGc)
            {
                GC.EndNoGCRegion();
            }

            // Throw away anything that might be in the receive buffer
            _interface.ClearReceiveBuffer();

            System.Diagnostics.Debug.WriteLine("Reading sync byte");
            var syncByte = _interface.ReadByte();
            if (syncByte != 0x55)
            {
                throw new InvalidOperationException(
                    $"Unexpected sync byte: Expected $55, Actual ${syncByte:X2}");
            }

            var keywordLsb = _interface.ReadByte();
            System.Diagnostics.Debug.WriteLine($"Keyword LSB: 0x{keywordLsb:X2}");

            var keywordMsb = ReadAndAckByte();

            System.Diagnostics.Debug.WriteLine($"Keyword MSB: 0x{keywordMsb:X2}");

            var protocolVersion = ((keywordMsb & 0x7F) << 7) + (keywordLsb & 0x7F);

            return protocolVersion;
        }

        public byte ReadByte()
        {
            return _interface.ReadByte();
        }

        public void WriteByte(byte b)
        {
            WriteByteAndDiscardEcho(b);
        }

        public byte ReadAndAckByte()
        {
            var b = _interface.ReadByte();
			WriteComplement(b);

			return b;
        }

        public void ReadComplement(byte b)
        {
            var expectedComplement = (byte)~b;
            var actualComplement = _interface.ReadByte();
            if (actualComplement != expectedComplement)
            {
                throw new InvalidOperationException(
                    $"Received complement ${actualComplement:X2} but expected ${expectedComplement:X2}");
            }
        }

        private void WriteComplement(byte b)
        {
            var complement = (byte)~b;
            WriteByteAndDiscardEcho(complement);
        }

        private void BitBang5Baud(byte b, bool evenParity)
        {
            const int bitsPerSec = 5;
            long ticksPerBit = Stopwatch.Frequency / bitsPerSec;

            long maxTick;

            // Delay the appropriate amount and then set/clear the TxD line
            void BitBang(bool bit)
            {
                while (Stopwatch.GetTimestamp() < maxTick)
                    ;
                if (bit)
                {
                    _interface.SetBreakOff();
                }
                else
                {
                    _interface.SetBreakOn();
                }

                maxTick += ticksPerBit;
            }

            bool parity = !evenParity; 

            maxTick = Stopwatch.GetTimestamp();
            BitBang(false); // Start bit

            for (int i = 0; i < 7; i++)
            {
                bool bit = (b & 1) == 1;
                parity ^= bit;
                b >>= 1;

                BitBang(bit);
            }

            BitBang(parity);

            BitBang(true); // Stop bit

            // Wait for end of stop bit
            while (Stopwatch.GetTimestamp() < maxTick)
                ;
        }

        /// <summary>
        /// Write a byte to the interface and read/discard its echo.
        /// </summary>
        private void WriteByteAndDiscardEcho(byte b)
        {
            _interface.WriteByteRaw(b);
			var echo = _interface.ReadByte();
            if (echo != b)
            {
                throw new InvalidOperationException($"Wrote 0x{b:X2} to port but echo was 0x{echo:X2}");
            }
        }

        private readonly IInterface _interface;

        public KwpCommon(IInterface @interface)
        {
            _interface = @interface;
        }
    }
}