using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Threading;

namespace AtariRobot
{
    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;

        protected byte[] SendMessage(string message)
        {
            //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)

            byte[] messageBytes = Encoding.ASCII.GetBytes(message);
            int messageStringLength = messageBytes.Length + 1;

            byte[] messageHeaderBytes = new byte[] { (byte)(4 + messageStringLength), 0x00, 0x80, 0x09, 0x00 };
            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);

            //return ReadData();
            return null;
        }

        protected bool GetCTS()
        {
            //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

            byte[] messageRequestBytes = new byte[] { 0x05, 0x00, 0x00, 0x13, 0x00, 0x00, 0x00 };
            m_port.Write(messageRequestBytes, 0, messageRequestBytes.Length);

            byte[] response = ReadData();
            
            return response[2] != 0;
        }

        protected 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;
        }

        protected RobotCommand m_lastCommand = RobotCommand.Stop;
        public void SendCommand(RobotCommand command)
        {
            if (command == m_lastCommand)
            {
                //Console.WriteLine("Skip: " + command);
                return;
            }

            m_lastCommand = command;

            Console.WriteLine(command);
            //if (GetCTS())
            //{
                SendMessage(command.ToString());
            //}
        }

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

        protected sbyte lastPower = -127;
        public void SendMotorCommand(byte motorPort, sbyte power, MotorMode motorMode, RegulationMode regulationMode, sbyte turnRatio, MotorRunState motorRunState)
        {
            if (lastPower == power)
            {
                return;
            }

            lastPower = power;

            //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[] motorCommand = new byte[] { 0x80, 0x04, motorPort, (byte)power, (byte)motorMode, (byte)regulationMode, (byte)turnRatio, (byte)motorRunState, 0x00, 0x00, 0x00, 0x00 };
            byte[] messageSize = new byte[] { (byte)motorCommand.Length, 0x00 };
            m_port.Write(messageSize, 0, messageSize.Length);
            m_port.Write(motorCommand, 0, motorCommand.Length);
        }

    }

    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
    }

    public class NxcMovementPacket
    {
        public NxcMotorPort MotorPort { get; set; }
        public sbyte Power { get; set; }
        public short Degrees { get; set; }
        public bool Brake { get; set; }

        public byte[] ToBytes()
        {
            byte[] bytes = new byte[] { (byte)MotorPort, (byte)Power, (byte)(Degrees % 256), (byte)(Degrees / 256), (byte)(Brake ? 0x01 : 0x00) };
            return bytes;
        }
    }

    public enum NxcMotorPort
    {
        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,
        OK = 0x00,
        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 NxtBluetooth2
    {
        public NxtBluetooth2(string comPort)
        {
            m_port = new SerialPort();
            m_port.PortName = comPort;
            m_port.ReadTimeout = 1000;
            m_port.WriteTimeout = 1000;
            m_port.Open();
        }

        public void Close()
        {
            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;
        }
    }

}