using System;

using Microsoft.SPOT;
using Microsoft.SPOT.Input;
using Microsoft.SPOT.Presentation;
using Microsoft.SPOT.Presentation.Controls;
using Microsoft.SPOT.Hardware;

using SPL.Script.Engine;

namespace SPLMFConsoleWindow1
{
    public class Program : Microsoft.SPOT.Application
    {
        public static void Main()
        {
            Program myApplication = new Program();

            Window mainWindow = myApplication.CreateWindow();

            GPIOButtonInputProvider inputProvider = new GPIOButtonInputProvider(null);

            myApplication.Run(mainWindow);
        }

        private Window mainWindow;

        //##########################  Sample Code  #################################
        private ConsoleWindow _consoleWindow;

        private I2CDevice _device = null;
        //##########################################################################

        public Window CreateWindow()
        {
            mainWindow = new Window();
            mainWindow.Height = SystemMetrics.ScreenHeight;
            mainWindow.Width = SystemMetrics.ScreenWidth;

            //##########################  Sample Code  #################################
            Font font = Resources.GetFont(Resources.FontResources.small);
            _consoleWindow = new ConsoleWindow(font);

            mainWindow.Child = _consoleWindow;
            //##########################################################################

            mainWindow.AddHandler(Buttons.ButtonUpEvent, new ButtonEventHandler(OnButtonUp), false);

            mainWindow.Visibility = Visibility.Visible;

            Buttons.Focus(mainWindow);

            //##########################  Sample Code  #################################
            _consoleWindow.WriteLine("Up button -> Go Forwards");
            _consoleWindow.WriteLine("Down button -> Go Backwards");
            _consoleWindow.WriteLine("Left button -> Turn Left");
            _consoleWindow.WriteLine("Right button -> Turn Right");
            _consoleWindow.WriteLine("Select button -> Stop");
            //##########################################################################


            //##########################  Sample Code  #################################
            I2CDevice.Configuration config = new I2CDevice.Configuration(
                            58,
                            100
                            );

            _device = new I2CDevice(config);
            //##########################################################################

            return mainWindow;
        }

        private void OnButtonUp(object sender, ButtonEventArgs e)
        {
            //##########################  Sample Code  #################################
            if (e.Button == Microsoft.SPOT.Hardware.Button.VK_SELECT)
            {
                //Stop

                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 100;     //100-100
                wBuff[2] = 100;

                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };

                int count = _device.Execute(wTrArr, 1000);
            }
            else if (e.Button == Microsoft.SPOT.Hardware.Button.VK_UP)
            {
                //Go Forwards

                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 120;     //120-100
                wBuff[2] = 120;

                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };

                int count = _device.Execute(wTrArr, 1000);
            }
            else if (e.Button == Microsoft.SPOT.Hardware.Button.VK_DOWN)
            {
                //Go Backwards

                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 80;
                wBuff[2] = 80;

                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };

                int count = _device.Execute(wTrArr, 1000);
            }
            else if (e.Button == Microsoft.SPOT.Hardware.Button.VK_LEFT)
            {
                //Turn Left

                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 90;
                wBuff[2] = 110;

                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };

                int count = _device.Execute(wTrArr, 1000);
            }
            else if (e.Button == Microsoft.SPOT.Hardware.Button.VK_RIGHT)
            {
                //Turn Right

                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 100;
                wBuff[2] = 90;

                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };

                int count = _device.Execute(wTrArr, 1000);
            }
            //##########################################################################
        }
    }
}

MFEmulator    emul1
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/BasicObstacles.xml"

AddDifferentialDriveEntity    base1

FlushScript  

Procedure  proc_i2c_write
	
	if (value.Id == "i2c1")
	{
		command_type = value.ReceivedBytes[0]
		
		if (command_type == 1)
		{		
			distance = value.ReceivedBytes[1]
			power = value.ReceivedBytes[2]
	
			power = power - 100

			distance = Util.ToDouble(distance / 10.0)
			power = Util.ToDouble(power / 100.0)

			print "Distance -> " + distance.ToString() + " m / Power -> " + power.ToString()
			
			base1.GoTo(distance, power)
		}
		else if (command_type == 2)
		{		
			degrees = value.ReceivedBytes[1]
			power = value.ReceivedBytes[2]
	
			degrees = degrees - 100
			power = power - 100

			degrees = Util.ToDouble(degrees)
			power = Util.ToDouble(power / 100.0)

			print "Degrees -> " + degrees.ToString() + " / Power -> " + power.ToString()
			
			base1.Turn(degrees, power)
		}
		else if (command_type == 4)
		{		
			left_power = value.ReceivedBytes[1]
			right_power = value.ReceivedBytes[2]
	
			left_power = left_power - 100
			right_power = right_power - 100

			left_power = Util.ToDouble(left_power / 100.0)
			right_power = Util.ToDouble(right_power / 100.0)

			print "Left -> " + left_power.ToString() + " / Right -> " + right_power.ToString()
			
			base1.Go(left_power, right_power)
		}
	}

End