using System;
using System.Linq;
namespace FreeD
{
    [Serializable]
    public class Packet
    {

        // From https://github.com/stvmyr/freeD
        //Offset    Function    Description
        //0	        Identifier  Message Type.The Encode function always uses 0xD1. (see freeD manual section A.3.1 for further information)
        //1	        ID          Camera ID.This is a relict when using multiple Systems via RS232 or RS422.
        //2:5	    Pitch       Camera Pitch angle described in degree.
        //5:8	    Yaw         Camera Yaw angle described in degree.
        //8:11	    Roll        Camera Roll angle described in degree.
        //11:14	    Position Z  Camera Z Offset from origin.Typically described in millimeter.
        //14:17	    Position Y  Camera Y Offset from origin.Typically described in millimeter.
        //17:20	    Position X  Camera X Offset from origin.Typically described in millimeter.
        //20:23	    Zoom Lens   Zoom Position.Typically measured with an external encoder attached to the Lens.In the most cases this is a value between 0-4095.
        //23:26	    Focus Lens  Focus Position. Typically measured with an external encoder attached to the Lens. In the most cases this is a value between 0-4095.
        //26:28	    Reserved    Currently not used in freeD.
        //28	    Checksum    Checksum of the first 28 bytes.The Decode function uses the checksum to verify if the incoming data is a valid freeD package.

        public int Id;
        public float Pan;
        public float Tilt;
        public float Roll;
        public float PosZ;
        public float PosX;
        public float PosY;
        public float Zoom;
        public float Focus;

        public static Packet Decode(byte[] data)
        {
            //if (Checksum(data) == data[28])
            {
                var trackingData = new Packet
                {
                    Id = data.Skip(1).First(),
                    Pan = GetRotation(data.Skip(2).Take(3).ToArray()),
                    Tilt = GetRotation(data.Skip(5).Take(3).ToArray()),
                    Roll = GetRotation(data.Skip(8).Take(3).ToArray()),
                    PosZ = GetPosition(data.Skip(11).Take(3).ToArray()),
                    PosX = GetPosition(data.Skip(14).Take(3).ToArray()),
                    PosY = GetPosition(data.Skip(17).Take(3).ToArray()),
                    Zoom = GetPosition(data.Skip(20).Take(3).ToArray()),
                    Focus = GetPosition(data.Skip(23).Take(3).ToArray())
                };

                return trackingData;
            }
            throw new Exception("Calculated checksum does not match provided data. Probably not FreeD.");
        }

        public static byte[] Encode(Packet data)
        {
            byte[] output = { 0xD1 };  // Identifier
            //output = output.Concat(new byte[] { 0xFF }).ToArray();  // ID
            output = output.Concat(SetCameraId(data.Id)).ToArray();  // ID: LQ
            output = output.Concat(SetRotation(data.Pan)).ToArray();  // Pitch
            output = output.Concat(SetRotation(data.Tilt)).ToArray();  // Yaw
            output = output.Concat(SetRotation(data.Roll)).ToArray();  // Roll
            output = output.Concat(SetPosition(data.PosZ)).ToArray();  // X
            output = output.Concat(SetPosition(data.PosX)).ToArray();  // Y
            output = output.Concat(SetPosition(data.PosY)).ToArray();  // Z
            output = output.Concat(SetPosition(data.Zoom)).ToArray();  // Zoom
            output = output.Concat(SetPosition(data.Focus)).ToArray();  // Focus
            output = output.Concat(new byte[] { 0x00, 0x00 }).ToArray();  // Reserved
            output = output.Concat(new byte[] { (byte)Checksum(output) }).ToArray();  // Checksum

            return output;
        }

        public static byte[] SetCameraId(int cameraId)
        {
            int id = (UInt16)(cameraId);
            byte[] data = BitConverter.GetBytes(id);
            if (!BitConverter.IsLittleEndian)
                Array.Reverse(data);
            return data.Skip(0).Take(1).ToArray();
        }

        public static byte[] SetPosition(float pos)
        {
            long position = (long)(pos * 64 * 256);
            byte[] data = BitConverter.GetBytes(position);
            Array.Reverse(data);
            return data.Skip(4).Take(3).ToArray();
        }

        public static byte[] SetRotation(float rot)
        {
            long rotation = (long)(rot * 32768 * 256);
            byte[] data = BitConverter.GetBytes(rotation);
            Array.Reverse(data);
            return data.Skip(4).Take(3).ToArray();
        }

        public static byte[] SetEncoder(int enc)
        {
            byte[] data = BitConverter.GetBytes(enc);
            Array.Reverse(data);
            return new byte[] { 0x00 }.Concat(data.Skip(5).Take(2)).ToArray();
        }

        public static float GetPosition(byte[] data)
        {
            return (float)(BitConverter.ToInt32(new byte[] { 0x00, data[2], data[1], data[0] }, 0) / 64 / 256);
        }

        public static float GetRotation(byte[] data)
        {
            return (float)(BitConverter.ToInt32(new byte[] { 0x00, data[2], data[1], data[0] }, 0) / 32768f / 256f);
        }

        public static int GetEncoder(byte[] data)
        {
            byte[] value = new byte[] { 0x00 }.Concat(data).ToArray();
            Array.Reverse(value);
            return BitConverter.ToInt32(value, 0);
        }

        public static int Checksum(byte[] data)
        {
            int sum = 64;
            for (int i = 0; i < 28; i++)
            {
                sum -= data[i];
            }
            return Modulo(sum, 256);
        }

        public static int Modulo(int d, int m)
        {
            int res = d % m;
            if ((res < 0 && m > 0) || (res > 0 && m < 0))
            {
                return res + m;
            }
            return res;
        }
    }
}