using System;
using System.IO.Ports;
namespace Robot
{
    enum CommandEnum{
        Unknown = 0,
        ReleaseAllServos = 1,
        SetSerialMacro = 2,
        SetI2CMacro = 3,
        WriteConfiguration = 4,
        ReadConfiguration = 5,
        StatusLED = 6,
        I2CStart = 7,
        I2CRestart = 8,
        I2CStop = 9,
        I2CWrite = 10,
        I2CRead = 11,
        I2CReadAutoAck = 12,
        I2CPoll = 13,
        BootLoader = 14,
        SetPWMSpeed = 15,
        BV4113_Cmd = 35,
        SetServoSpeed = 40,
        Ping = 85,
        SetDigitalPortOn = 100,
        SetDigitalPortOff = 120,
        GetDigitalPort = 140,
        SetServoPosition = 160,
        GetADCValue = 180,
        SendSerial = 190,
        HC_SR04 = 210,
        PlayNote = 230,
    };

    class MainClass
    {
        static SerialPort connection;

        public static void Main (string[] args)
        {

            connection = new SerialPort ("COM3");
            connection.Open ();
            byte[] buffer = new byte[1];
            buffer [0] = (byte)CommandEnum.Ping;


            Console.WriteLine ("GOING TO WRITE");

            string writing = "";
            while (!writing.Equals("quit")) {
                writing = Console.ReadLine();
                connection.Write(buffer, 0, 1);
                Console.WriteLine ("WROTE");
                byte[] readBuffer = new byte[1];
                connection.Read(readBuffer, 0, 1);

                Console.WriteLine("READ " + readBuffer[0]);

            }




            Console.Read();
            Console.Read();
        }
    }
}