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 System.IO.Ports.SerialPort _serialPort = 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 go forwards.");
            //##########################################################################


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

            _device = new I2CDevice(config);


            _serialPort = new System.IO.Ports.SerialPort("COM3", 9600);
            _serialPort.Open();

            System.Threading.Thread serialReaderThread = new System.Threading.Thread(SerialPortReader);
            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] = 120;     //120-100
                wBuff[2] = 120;

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

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

                _consoleWindow.WriteLine("Go forwards");
            }
            //##########################################################################
        }


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

            while (true)
            {
                try
                {
                    int rCnt = _serialPort.Read(rBuff, 0, 361);

                    if (rCnt == 361)
                    {
                        LogInfo("Distance of 180th is " + (rBuff[180] * 32).ToString());
                    }
                }
                catch { }                
            }
        }

        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/basicenv4.xml"

AddDifferentialDriveEntity    base1

AddLaserRangeFinderEntity    lrf1
	/Position:0  0.4  0
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_lrf

FlushScript  

sendBytes = Util.CreateArrayByte(361)

skip_count = 4

ready_flag = false

procedure  proc_lrf
	
	if (skip_count <= 0 && ready_flag)
	{
		lrfbuff = value.DistanceMeasurements
	
		for (i = 0; i < 361; i++)
		{
			//convert scalse to byte 8000 -> 250
			distance = lrfbuff[i] / 32
			sendBytes[i] = Util.ToByte(distance)
		}
		
		emul1.SendToSerialPort("COM3", sendBytes)
		
		d180 = lrfbuff[180]
		print d180
		
		skip_count = 4
	}
	
	skip_count--
end 

procedure  proc_i2c_write
	
	ready_flag = true

	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