using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
using System.Diagnostics;

namespace BluetoothSandbox
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();

            Bluetooth = new NxtBluetooth("Com5");
        }

        NxtBluetooth Bluetooth { get; set; }


        private void stopButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, 0);
        }

        private void leftButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, -30);
        }

        private void leftMedButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, -20);
        }

        private void leftSlowButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, -15);
        }

        private void rightButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, 30);
        }

        private void rightMedButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, 20);
        }

        private void rightSlowButton_Click(object sender, EventArgs e)
        {
            Bluetooth.SendMotorCommand(0, 15);
        }

        private void goButton_Click(object sender, EventArgs e)
        {
            //MotorMode motorMode = MotorMode.None;
            //if (modeOnCheck.Checked) { motorMode |= MotorMode.MotorOn; }
            //if (modeBrakeCheck.Checked) { motorMode |= MotorMode.Brake; }
            //if (modeRegulateCheck.Checked) { motorMode |= MotorMode.Regulated; }
            //Bluetooth.ResetMotorPosition(0);
            //Bluetooth.SendMotorCommand(0, sbyte.Parse(powerBox.Text), motorMode, RegulationMode.Idle, sbyte.Parse(turnRatioBox.Text), MotorRunState.Running, uint.Parse(tachoLimitBox.Text));

            //byte[] bytes = null;
            //bytes = Bluetooth.SendMessage(0, new byte[] { 1 }, true);
            //bytes = Bluetooth.SendMessage(0, BitConverter.GetBytes(int.Parse(tachoLimitBox.Text)), true);
            //bytes = Bluetooth.SendMessage(0, "Go!", true);

            ProcessMotorPacket((NxcMotorPorts)Enum.Parse(typeof(NxcMotorPorts), turnRatioBox.Text), sbyte.Parse(powerBox.Text), short.Parse(tachoLimitBox.Text));
            ProcessMotorPacket((NxcMotorPorts)Enum.Parse(typeof(NxcMotorPorts), turnRatioBox.Text), (sbyte)-sbyte.Parse(powerBox.Text), short.Parse(tachoLimitBox.Text));
        }

        public void ProcessMotorPacket(NxcMotorPorts port, sbyte power, short degrees)
        {
            NxcMovementPacket packet = new NxcMovementPacket();
            packet.MotorPort = port;
            packet.Power = power;
            packet.Degrees = degrees;

            Bluetooth.SendMessage(0, packet.ToBytes(), false);

            byte[] bytes = null;
            do
            {
                bytes = Bluetooth.ReadMailbox(0, false);
            }
            while (NxtResponsePacket.ReadStatus(bytes) == NxtResponseCode.MailboxEmpty || !NxtMailbox.ReadLogic(bytes));

        }
    }

    public class NxcMovementPacket
    {
        public NxcMotorPorts MotorPort { get; set; }
        public sbyte Power { get; set; }
        public short Degrees { get; set; }

        public byte[] ToBytes()
        {
            byte[] bytes = new byte[] { (byte)MotorPort, (byte)Power, (byte)(Degrees % 256), (byte)(Degrees / 256) };
            return bytes;
        }
    }


    public enum NxcMotorPorts
    {
        OUT_A = 0,
        OUT_B = 1,
        OUT_C = 2,
        OUT_AB = 3,
        OUT_AC = 4,
        OUT_BC = 5,
        OUT_ABC = 6
    }


    public static class NxtMailbox
    {
        public static bool ReadLogic(byte[] mailboxResponse)
        {
            return mailboxResponse[5] == 1;
        }
    }

    public static class NxtResponsePacket
    {
        public static NxtResponseCode ReadStatus(byte[] response)
        {
            return (NxtResponseCode)response[2];
        }
    }

    public enum NxtResponseCode
    {
        Unknown = -1,
        PendingInProgress = 0x20,
        MailboxEmpty = 0x40,
        RequestFailed = 0xBD,
        UnknownOpcode = 0xBE,
        InsanePacket = 0xBF,
        DataOutOfRange = 0xC0,
        CommBusError = 0xDD,
        NoFreeMemoryInBuffer = 0xDE,
        ChannelInvalid = 0xDF,
        ChannelBusy = 0xE0,
        NoActiveProgram = 0xEC,
        IllegalSize = 0xED,
        IllegalMailbox = 0xEE,
        InvalidField = 0xEF,
        BadIO = 0xF0,
        OutOfMemory = 0xFB,
        BadArguments = 0xFF
    }


    public class NxtBluetooth
    {
        public NxtBluetooth(string comPort)
        {
            m_port = new SerialPort();
            m_port.PortName = comPort;
            m_port.ReadTimeout = 1000;
            m_port.WriteTimeout = 1000;
            m_port.Open();
        }

        public void Close()
        {
            SendCommand(RobotCommand.Stop);
            m_port.Close();
        }

        protected SerialPort m_port;

        public byte[] SendMessage(byte mailbox, byte[] messageBytes)
        {
            return SendMessage(mailbox, messageBytes, true);
        }

        public byte[] SendMessage(byte mailbox, byte[] messageBytes, bool waitForResponse)
        {
            //Byte 0: Message Size LSB
            //Byte 1: Message Size MSB
            //Byte 2: 0x00 
            //Byte 3: 0x09 
            //Byte 4: 0-Based Mailbox Number 
            //Byte 5: Data + \0 Length
            //Byte 6-N: Command. 
            //Byte N+1: 0x00 (terminator)

            int messageStringLength = messageBytes.Length + 1;

            byte[] messageHeaderBytes = new byte[] { (byte)(4 + messageStringLength), 0x00, (byte)(waitForResponse ? 0x00 : 0x80), 0x09, mailbox };
            byte[] messageLengthBytes = new byte[] { (byte)(messageBytes.Length + 1) };
            byte[] messageTerminatorBytes = new byte[] { 0x00 };

            m_port.Write(messageHeaderBytes, 0, messageHeaderBytes.Length);
            m_port.Write(messageLengthBytes, 0, messageLengthBytes.Length);
            m_port.Write(messageBytes, 0, messageBytes.Length);
            m_port.Write(messageTerminatorBytes, 0, messageTerminatorBytes.Length);

            if (waitForResponse)
            {
                return ReadData();
            }
            else
            {
                return null;
            }
        }

        public byte[] SendMessage(byte mailbox, string message)
        {
            return SendMessage(mailbox, message, true);
        }

        public byte[] SendMessage(byte mailbox, string message, bool waitForResponse)
        {
            return SendMessage(mailbox, Encoding.ASCII.GetBytes(message), waitForResponse);
        }

        public byte[] ReadMailbox(byte mailbox, bool peek)
        {
            //Byte 0: Message Size LSB
            //Byte 1: Message Size MSB
            //Byte 2: 0x00 
            //Byte 3: 0x13 
            //Byte 4: 10-Based Mailbox Number 
            //Byte 5: 0x00
            //Byte 6: 0x01 clears, 0x00 peeks

            byte[] messageRequestBytes = new byte[] { 0x05, 0x00, 0x00, 0x13, (byte)(0x0A + mailbox), 0x00, (byte)(peek?0x00:0x01) };
            m_port.Write(messageRequestBytes, 0, messageRequestBytes.Length);

            return ReadData();
        }


        public byte[] ReadData()
        {
            int length = m_port.ReadByte() + 256 * m_port.ReadByte();

            byte[] bytes = new byte[length];
            for (int bytesRead = 0; bytesRead < length; bytesRead += m_port.Read(bytes, bytesRead, length - bytesRead)) { }

            return bytes;
        }

        public void SendMotorCommand(byte motorPort, sbyte power)
        {
            SendMotorCommand(motorPort, power, MotorMode.Brake | MotorMode.MotorOn | MotorMode.Regulated, RegulationMode.Idle, 0, MotorRunState.Running, 0);
        }

        public void SendMotorCommand(byte motorPort, sbyte power, MotorMode motorMode, RegulationMode regulationMode, sbyte turnRatio, MotorRunState motorRunState, uint tachoLimit)
        {
            //Byte 0 = Size LSB
            //Byte 1 = Size MSB

            //Byte 0: 0x00 or 0x80 (Send Response Flag)
            //Byte 1: 0x04  Command
            //Byte 2: Motor port
            //Byte 3: Power: -100 - 100
            //Byte 4: Mode
            //Byte 5: Regulation Mode
            //Byte 6: Turn Ratio: -100 - 100
            //Byte 7: Run State
            //Byte 8-12: Tacho Limit

            byte[] tachoLimitBytes = BitConverter.GetBytes(tachoLimit);

            byte[] motorCommand = new byte[] { 0x80, 0x04, motorPort, (byte)power, (byte)motorMode, (byte)regulationMode, (byte)turnRatio, (byte)motorRunState, tachoLimitBytes[0], tachoLimitBytes[1], tachoLimitBytes[2], tachoLimitBytes[3] };
            byte[] messageSize = new byte[]{ (byte) motorCommand.Length, 0x00};
            m_port.Write(messageSize, 0, messageSize.Length);
            m_port.Write(motorCommand, 0, motorCommand.Length);
        }

        public void ResetMotorPosition(byte motorPort)
        {
            //Byte 0 = Size LSB
            //Byte 1 = Size MSB

            //Byte 0: 0x00 or 0x80 (Send Response Flag)
            //Byte 1: 0x0A  Command
            //Byte 2: Motor port
            //Byte 3: Relative

            byte[] motorCommand = new byte[] { 0x80, 0x0A, motorPort, (byte)0 };
            byte[] messageSize = new byte[] { (byte)motorCommand.Length, 0x00 };
            m_port.Write(messageSize, 0, messageSize.Length);
            m_port.Write(motorCommand, 0, motorCommand.Length);
        }

        public void SendCommand(RobotCommand command)
        {
            SendMessage(0, command.ToString());
        }
    }

    public enum RobotCommand
    {
        Stop,
        LeftFast,
        LeftMedium,
        LeftSlow,
        RightFast,
        RightMedium,
        RightSlow
    }

    public enum MotorMode
    {
        None = 0x00,
        MotorOn = 0x01,
        Brake = 0x02,
        Regulated = 0x04
    }

    public enum RegulationMode
    {
        Idle = 0x00,
        Speed = 0x01,
        Sync = 0x02
    }

    public enum MotorRunState
    {
        Idle = 0x00,
        RampUp = 0x10,
        Running = 0x20,
        RampDown = 0x40
    }
}