private static NetworkStream iStream;
private static StringBuilder iCommandBuilder;
private static bool iCommandFlush = true;
private static int iTestSpeedL = 30;
private static int iTestSpeedR = 30;
private static Controller iController;

static void Main(string[] args)
{
    Console.WriteLine("Looking for Xbox controller...");
    iController = new Controller(UserIndex.One);
    if (iController.IsConnected)
    {
        Console.WriteLine("Xbox controller connected!");
    }
         
    iCommandBuilder = new StringBuilder();

    string lAvogadro = "provide your photon key";
    string lToken = "provide your token";
    string lMethodName = "connect";
    string lUrl = @"https://api.particle.io/v1/devices/" + lAvogadro + @"/" + lMethodName + "?access_token=" + lToken;
    int lPort = 13914;

    Console.WriteLine("Start listener...");
    TcpListener lListener = new TcpListener(lPort);
    lListener.Start();

    Console.WriteLine("Sending IP '" + GetOwnIP() + "'...");
    WebClient lWebClient = new WebClient();
    var lValues = new NameValueCollection();
    lValues.Add("args", GetOwnIP() + "." + lPort.ToString());
    lWebClient.UploadValues(lUrl, lValues);

    Console.Write("Waiting for a connection... ");
    TcpClient lClient = lListener.AcceptTcpClient();
    Console.WriteLine("Connected!");

    iStream = lClient.GetStream();

    int lAngle = 90;

    short lOldLeft = -10000;
    short lOldRight = -10000;

    bool lLed0 = false;
    bool lLed1 = false;
    bool lLed2 = false;
    bool lA = false;

    if (iController != null)
    {
        while (true)
        {
            var lState = iController.GetState();
            var lLeft = lState.Gamepad.LeftThumbY;
            var lRight = lState.Gamepad.RightThumbY;

            Console.WriteLine("Left: " + lLeft + " - Right: " + lRight);

            if (Math.Abs(lOldLeft - lLeft) + Math.Abs(lOldRight - lRight) > 5000)
            {
                BeginCommand();
                WriteCommand(ObjectEnum.ServoLeft, Convert.ToInt32(Math.Round(lLeft / 1000.0)));
                WriteCommand(ObjectEnum.ServoRight, Convert.ToInt32(Math.Round(lRight / 1000.0)));
                EndCommand();

                lOldLeft = lLeft;
                lOldRight = lRight;
            }
            if (lState.Gamepad.LeftTrigger == 255 && !lLed0)
            {
                string lCommand = "L01#";
                WriteCommand(lCommand);
                lLed0 = true;
            }
            else if (lState.Gamepad.LeftTrigger == 0 && lLed0)
            {
                string lCommand = "L00#";
                WriteCommand(lCommand);
                lLed0 = false;
            }
            if (lState.Gamepad.RightTrigger == 255 && !lLed2)
            {
                string lCommand = "L21#";
                WriteCommand(lCommand);
                lLed2 = true;
            }
            else if (lState.Gamepad.RightTrigger == 0 && lLed2)
            {
                string lCommand = "L20#";
                WriteCommand(lCommand);
                lLed2 = false;
            }
            if (lState.Gamepad.Buttons == GamepadButtonFlags.A && !lA)
            {
                WriteCommand("L01#W100#L11#W100#L21#W100#L00#W100#L10#W100#L20#W100#"
                    + "L21#W100#L11#W100#L01#W100#L20#W100#L10#W100#L00#W100#");
                //lState.Gamepad.Buttons = GamepadButtonFlags.None;
                lA = true;
            }
            if (lState.Gamepad.Buttons != GamepadButtonFlags.A)
            {
                lA = false;
            }
        }
    }

    Console.WriteLine("Give input...");
    var lKey = Console.ReadKey();

      

    while (lKey.KeyChar != 'c')
    {
        if (lKey.KeyChar == 'a')
        {
            //WriteCommand("SR10#SL170#W300#SR90#SL90#");
            BeginCommand();
            WriteCommand(ObjectEnum.ServoBoth, 50);
            WriteCommand(ObjectEnum.Wait, 300);
            WriteCommand(ObjectEnum.ServoBoth, 0);
            EndCommand();
        }
        else if (lKey.KeyChar == 'z')
        {
            //WriteCommand("SR170#SL10#W300#SR90#SL90#");
            BeginCommand();
            WriteCommand(ObjectEnum.ServoBoth, -50);
            WriteCommand(ObjectEnum.Wait, 300);
            WriteCommand(ObjectEnum.ServoBoth, 0);
            EndCommand();
        }
        else if (lKey.KeyChar == 'o')
        {
            //WriteCommand("SR170#SL170#W100#SR90#SL90#");
            BeginCommand();
            WriteCommand(ObjectEnum.ServoLeft, -50);
            WriteCommand(ObjectEnum.ServoRight, 50);
            WriteCommand(ObjectEnum.Wait, 300);
            WriteCommand(ObjectEnum.ServoBoth, 0);
            EndCommand();
        }
        else if (lKey.KeyChar == 'p')
        {
            //WriteCommand("SR10#SL10#W100#SR90#SL90#");
            BeginCommand();
            WriteCommand(ObjectEnum.ServoLeft, 50);
            WriteCommand(ObjectEnum.ServoRight, -50);
            WriteCommand(ObjectEnum.Wait, 300);
            WriteCommand(ObjectEnum.ServoBoth, 0);
            EndCommand();
        }
        else if (lKey.KeyChar == '3')
        {
            string lCommand = "L0";
            if (lLed0) lCommand += "0#";
            else lCommand += "1#";
            WriteCommand(lCommand);
            lLed0 = !lLed0;
        }
        else if (lKey.KeyChar == '2')
        {
            string lCommand = "L1";
            if (lLed1) lCommand += "0#";
            else lCommand += "1#";
            WriteCommand(lCommand);
            lLed1 = !lLed1;
        }
        else if (lKey.KeyChar == '1')
        {
            string lCommand = "L2";
            if (lLed2) lCommand += "0#";
            else lCommand += "1#";
            WriteCommand(lCommand);
            lLed2 = !lLed2;
        }
        else if (lKey.KeyChar == '4')
        {
            WriteCommand("L01#W100#L11#W100#L21#W100#L00#W100#L10#W100#L20#W100#"
                + "L21#W100#L11#W100#L01#W100#L20#W100#L10#W100#L00#W100#");
        }
        else if (lKey.KeyChar == 'h')
        {
            iTestSpeedR += 1;
            Console.WriteLine("TestspeedR " + iTestSpeedR.ToString());
            WriteCommand(ObjectEnum.ServoRight, iTestSpeedR);
        }
        else if (lKey.KeyChar == 'n')
        {
            iTestSpeedR -= 1;
            Console.WriteLine("iTestSpeedR " + iTestSpeedR.ToString());
            WriteCommand(ObjectEnum.ServoRight, iTestSpeedR);
        }
        else if (lKey.KeyChar == 'g')
        {
            iTestSpeedL += 1;
            Console.WriteLine("iTestSpeedL " + iTestSpeedL.ToString());
            WriteCommand(ObjectEnum.ServoLeft, iTestSpeedL);
        }
        else if (lKey.KeyChar == 'b')
        {
            iTestSpeedL -= 1;
            Console.WriteLine("iTestSpeedL " + iTestSpeedL.ToString());
            WriteCommand(ObjectEnum.ServoLeft, iTestSpeedL);
        }

        lKey = Console.ReadKey();

        while (iStream.DataAvailable)
        {
            // Read the first batch of the TcpServer response bytes.
            Byte[] lDataBuffer = new Byte[256];
            Int32 lBytes = iStream.Read(lDataBuffer, 0, lDataBuffer.Length);
            var lData = System.Text.Encoding.ASCII.GetString(lDataBuffer, 0, lBytes);
            Console.WriteLine("Received: {0}", lData);
        }
    }

    // Close everything.
    iStream.Close();
    lClient.Close();
}

private static void EndCommand()
{
    iCommandFlush = true;
    FlushCommand();
}
private static void FlushCommand()
{
    if (iCommandFlush)
    {
        Byte[] lD = System.Text.Encoding.ASCII.GetBytes(iCommandBuilder.ToString());
        iStream.Write(lD, 0, lD.Length);
        iCommandBuilder.Clear();
    }
}

private static void BeginCommand()
{
    iCommandFlush = false;
}

private static void WriteCommand(ObjectEnum pObject, int pValue)
{
    switch (pObject)
    {
        case ObjectEnum.ServoLeft:
            WriteCommand("SL" + pValue.ToString());
            break;
        case ObjectEnum.ServoRight:
            WriteCommand("SR" + pValue.ToString());
            break;
        case ObjectEnum.ServoBoth:
            WriteCommand("SX" + pValue.ToString());
            break;
        /*case ObjectEnum.Led0:                    
        case ObjectEnum.Led1:                    
        case ObjectEnum.Led2:
        */
        case ObjectEnum.Wait:
            WriteCommand("W" + pValue.ToString());
            break;
        default:
            throw new Exception("Invalid command!");
    }
}

private static float iSpeedZero = -1.0F;
private static float iSpeedRange = 1.2F;

private static float iDirectionZero = 0.02F;
private static float iDirectionFactor = 10F;


private static void WriteCommand(string pCommand)
{
    if (iStream == null || !iStream.CanWrite) return;

    if (pCommand.EndsWith("#"))
    {
        iCommandBuilder.Append(pCommand);
    }
    else
    {
        iCommandBuilder.Append(pCommand + "#");
    }
    FlushCommand();
}

private static string GetOwnIP()
{
    IPHostEntry host;
    host = Dns.GetHostEntry(Dns.GetHostName());
    foreach (IPAddress ip in host.AddressList)
    {
        if (ip.AddressFamily.ToString() == "InterNetwork")
        {
            return ip.ToString();
        }
    }
    return null;
}