using System;
using System.Collections.Generic;
using System.Text;

namespace NavNet
{
  public class RawPacket
  {
    #region Declarations

    public ushort SourceAddress { get; set; }
    public ushort DestinationAddress { get; set; }
    public ServiceId SourceServiceId { get; set; }
    public ServiceId DestinationServiceId { get; set; }
    public byte ACK { get; set; }
    public byte? Sequence { get; set; }
    public byte[] Data { get; set; }

    public enum ServiceId
    {
      NACK = 0x00D3,
      DeviceAddressAssignment = 0x4003,
      DeviceInformation = 0x4020,
      Login = 0x4050,
      InstallOS = 0x4080,
      ServiceDisconnection = 0x40DE,
    };

    #endregion

    #region Constructors / Teardown

    public RawPacket(byte[] header, byte[] data)
    {
      //Save the header data
      SourceAddress = (ushort)((header[2] << 8) | header[3]);
      SourceServiceId = (ServiceId)((header[4] << 8) | header[5]);
      DestinationAddress = (ushort)((header[6] << 8) | header[7]);
      DestinationServiceId = (ServiceId)((header[8] << 8) | header[9]);
      ACK = header[13];
      Sequence = header[14];

      //Save the packet data
      Data = data;
    }

    public RawPacket(ushort sourceAddress, ServiceId sourceServiceId, ushort destinationAddress, ServiceId destinationServiceId,
      byte ACK, byte[] data)
    {
      SourceAddress = sourceAddress;
      SourceServiceId = sourceServiceId;
      DestinationAddress = destinationAddress;
      DestinationServiceId = destinationServiceId;
      this.ACK = ACK;
      Data = data;
    }

    #endregion

    #region Public Methods

    public byte[] GetRawData(byte sequenceId)
    {
      const int HEADER_SIZE = 16;
      var ret = new byte[HEADER_SIZE + (Data != null ? Data.Length : 0x00)];

      ret[0] = 0x54;
      ret[1] = 0xFD;
      ret[2] = (byte)((SourceAddress >> 8) & 0xFF);
      ret[3] = (byte)(SourceAddress & 0xFF);
      ret[4] = (byte)(((ushort)SourceServiceId >> 8) & 0xFF);
      ret[5] = (byte)((ushort)SourceServiceId & 0xFF);
      ret[6] = (byte)((DestinationAddress >> 8) & 0xFF);
      ret[7] = (byte)(DestinationAddress & 0xFF);
      ret[8] = (byte)(((ushort)DestinationServiceId >> 8) & 0xFF);
      ret[9] = (byte)((ushort)DestinationServiceId & 0xFF);
      ret[12] = (byte)(Data != null ? Data.Length : 0x00);
      ret[13] = ACK;
      ret[14] = sequenceId;
      
      //Calculate checksums
      ushort checksum = _GetDataChecksum();
      ret[10] = (byte)((checksum >> 8) & 0xFF);
      ret[11] = (byte)(checksum & 0xFF);
      for (int i = 0; i < HEADER_SIZE - 1; i++)
        ret[15] += ret[i];

      //Fill in data
      if (Data != null && Data.Length > 0)
      {
        for (int i = 0; i < Data.Length; i++)
          ret[HEADER_SIZE + i] = Data[i];
      }

      return ret;
    }

    #endregion

    #region Private Methods

    private ushort _GetDataChecksum()
    {
      ushort ret = 0x0000;

      if (Data != null && Data.Length > 0)
      {
        foreach (byte b in Data)
        {
          ushort first = (ushort)((b << 8) | (ret >> 8));
          ret = (ushort)(ret & 0xFF);
          ushort second = (ushort)((((ret & 0x0F) << 4) ^ ret) << 8);
          ushort third = (ushort)(second >> 5);
          ret = (ushort)(third >> 7);
          ret = (ushort)((ret ^ first ^ second ^ third) & 0xFFFF);
        }
      }

      return ret;
    }

    #endregion
  }
}