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;
        private I2CDevice _i2c2 = 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("Push Select-Button to make a robot turn.");
            //##########################################################################


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

            _device = new I2CDevice(config);


            I2CDevice.Configuration i2c2_config = new I2CDevice.Configuration(
                            60,
                            100
                            );

            _i2c2 = new I2CDevice(i2c2_config);

            System.Threading.Thread serialReaderThread = new System.Threading.Thread(i2c2_Reader);
            serialReaderThread.Start();

            //##########################################################################

            return mainWindow;
        }

        private void OnButtonUp(object sender, ButtonEventArgs e)
        {
            //##########################  Sample Code  #################################
            if (e.Button == Microsoft.SPOT.Hardware.Button.VK_SELECT)
            {
                byte[] wBuff = new byte[3];
                wBuff[0] = 4;
                wBuff[1] = 90;     //-0.1
                wBuff[2] = 110;     //0.1

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

                //Go Forwards
                int count = _device.Execute(wTrArr, 1000);

                _consoleWindow.WriteLine("Turn left");
            }
            //##########################################################################
        }


        //##########################  Sample Code  #################################
        private void i2c2_Reader()
        {
            byte[] rBuff = new byte[3];

            while (true)
            {
                try
                {
                    I2CDevice.I2CReadTransaction rTr = _device.CreateReadTransaction(rBuff);
                    I2CDevice.I2CTransaction[] rTrArr = new I2CDevice.I2CTransaction[] { rTr };

                    int count = _i2c2.Execute(rTrArr, 1000);

                    if (rBuff != null)
                    {
                        int r = rBuff[0];
                        int g = rBuff[1];
                        int b = rBuff[2];

                        LogInfo("Detected color : " + r.ToString() + " / " + g.ToString() + " / " + b.ToString());
                    }
                }
                catch { }

                System.Threading.Thread.Sleep(300);
            }
        }

        private void LogInfo(string log)
        {
            this.Dispatcher.BeginInvoke(
                new DispatcherOperationCallback(
                    delegate(object report)
                    {
                        _consoleWindow.WriteLine(log);
                        return null;
                    }
                    ), null
                );
        }
        //##########################################################################
    }
}

MFEmulator    emul1
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicsim.xml"

AddNewEntity    ent1  	/Position:1.5  0.3  0
AddBoxShape  	/Dimensions:0.6  0.8  2.0  	/Mass:100
	/DiffuseColor:1  0  0  1

AddNewEntity    ent2  	/Position:0  0.4  -1.5
AddBoxShape  	/Dimensions:2.0  0.8  0.6  	/Mass:100
	/DiffuseColor:1  1  1  1

AddNewEntity    ent3  	/Position:-1.5  0.4  0
AddBoxShape  	/Dimensions:0.6  0.8  2.0  	/Mass:100
	/DiffuseColor:0  0  0.627451  1

AddDifferentialDriveEntity    base1

AddColorSensorEntity    color1
	/Position:0  0.4  -0.3
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_color

FlushScript  

procedure  proc_color

	r = value.NormalizedAverageRed	//0.0 ~ 1.0
	g = value.NormalizedAverageGreen	//0.0 ~ 1.0
	b = value.NormalizedAverageBlue	//0.0 ~ 1.0
	
	r = r * 255	//convert to 0 ~ 255
	g = g * 255	//convert to 0 ~ 255
	b = b * 255	//convert to 0 ~ 255
	
	//convert type as byte
	sendBytes = Util.CreateArrayByte(3)
	sendBytes[0] = Util.ToByte(r)	
	sendBytes[1] = Util.ToByte(g)	
	sendBytes[2] = Util.ToByte(b)	
		
	emul1.SetI2CReadBuffer("i2c2", sendBytes)
		
end 

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