using System.Collections;
using System.Runtime.InteropServices;
using System;
using System.IO.Ports;

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct UartCommandHeader {
    public uint id;
    public byte payloadSize;
}

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct UartCommand {
    public UartCommandHeader header;
    public const int MaxPayloadSize = 32;
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = MaxPayloadSize)]
    public byte[] payload;

    public UartCommand(uint _id, byte[] payloadBytes = null) {
        byte _payloadSize = 0;
        if (payloadBytes != null) {
            if (payloadBytes.Length > MaxPayloadSize) {
                throw new Exception("Too big payload!");
            }
            _payloadSize = (byte)payloadBytes.Length;
        }
        header = new UartCommandHeader() { id = _id, payloadSize = _payloadSize };
        payload = new byte[MaxPayloadSize];

        if (_payloadSize > 0) {
            Array.Copy(payloadBytes, payload, payloadBytes.Length);
        }
    }

    public UartCommand Write<T>(T val) where T : struct {
        var bytes = Utils.StructToBytes(val);
        for (int i = 0; i < bytes.Length; ++i) {
            payload[header.payloadSize++] = bytes[i];
        }
        return this;
    }

    public static UartCommand Create() {
        var result = new UartCommand();
        result.payload = new byte[MaxPayloadSize];
        return result;
    }

    public static UartCommand Create(uint id) {
        var result = Create();
        result.header.id = id;
        return result;
    }

    public UartCommand SetID(uint _id) {
        header.id = _id;
        return this;
    }

    public static int Size {
        get { return Marshal.SizeOf(typeof(UartCommandHeader)) + MaxPayloadSize; }
    }

    public UartCommandReader CreateReader() {
        return new UartCommandReader(ref this);
    }
}

public class UartCommandReader {
    private int offset = 0;
    private byte[] buffer;
    public UartCommandReader(ref UartCommand cmd) {
        buffer = cmd.payload;
    }

    public T Read<T>() where T : struct {
        var result = Utils.StructFromBytes<T>(buffer, offset);
        offset += Marshal.SizeOf(typeof(T));
        return result;
    }
}


public static class Utils {
    public static T StructFromBytes<T>(byte[] buffer, int offset = 0) where T : struct {
        var handle = GCHandle.Alloc(buffer, GCHandleType.Pinned);
        var result = (T)Marshal.PtrToStructure(new IntPtr(handle.AddrOfPinnedObject().ToInt64() + offset), typeof(T));
        handle.Free();
        return result;
    }

    public static byte[] StructToBytes<T>(T val) where T : struct {
        var result = new byte[Marshal.SizeOf(typeof(T))];
        var handle = GCHandle.Alloc(result, GCHandleType.Pinned);
        Marshal.StructureToPtr(val, handle.AddrOfPinnedObject(), false);
        handle.Free();
        return result;
    }
}

public static class SerialPortEx {
    public static UartCommand ExecuteCommand(this SerialPort _this, UartCommand cmd) {
        SendCommand(_this, cmd);
        return ReceiveCommand(_this);
    }

    private static void SendCommand(SerialPort _this, UartCommand cmd) {
        var bytes = Utils.StructToBytes(cmd);
        _this.Write(bytes, 0, bytes.Length);
        _this.BaseStream.Flush();
    }

    private static UartCommand ReceiveCommand(SerialPort _this) {
        var buffer = new byte[UartCommand.Size];
        int done = 0;
        while (done != buffer.Length) {
            var len = _this.Read(buffer, done, buffer.Length - done);
            if (len <= 0) {
                throw new Exception("Failed to read command");
            }
            done += len;
        }
        _this.BaseStream.Flush();
        return Utils.StructFromBytes<UartCommand>(buffer);
    }
}

public struct uart_commands {
    public static uint FOURCC(char a, char b, char c, char d) {
        return ((uint)((((uint)d) << 24) | (((uint)c) << 16) | (((uint)b) << 8) | ((uint)a)));
    }
    public static uint led_write = FOURCC('L', 'e', 'd', 'W');
    public static uint led_read = FOURCC('L', 'e', 'd', 'R');
    public static uint motor_write = FOURCC('M', 't', 'r', 'W');
    public static uint play_sound = FOURCC('P', 'S', 'n', 'd');
    public static uint acc_read = FOURCC('A', 'c', 'c', 'R');
    public static uint gyro_read = FOURCC('G', 'r', 'o', 'R');
};