MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/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 

I2CDevice    i2c1
	/Address:58
	/ClockRateKhz:100

//New I2C device for detection of IR sensor
I2CDevice    i2c2
	/Address:60
	/ClockRateKhz:100


colorBuff = Util.CreateArrayByte(3)

buff1 = Util.CreateArrayByte(3)

//turn left
buff1[0] = 4
buff1[1] = 90	//-0.1
buff1[2] = 110	//0.1

//make a robot move itselft
i2c1.Write(buff1)

//call procedure with the separate thread
call proc_read_color with concur

procedure proc_read_color

	bool stopFlag = false
	
	while(!stopFlag)
	{	
		i2c2.Read(colorBuff)
			
		print "Color: red " + colorBuff[0] + " / green " + colorBuff[1] + " / blue " + colorBuff[2]
		
		if (colorBuff[0] > 229 && colorBuff[1] < 76 && colorBuff[2] < 76)
		{	
			//stop
			buff1[0] = 4
			buff1[1] = 100	//0.0
			buff1[2] = 100	//0.0
			
			i2c1.Write(buff1)
			
			print "Found red box."
			
			stopFlag = true
		}
			
		wait 300
	}
	
end