MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicsim.xml"

AddNewEntity  whitepanel  	/Position:0  0.005  0

AddBoxShape
	/Dimensions:10  0.01  10  	/Mass:100
	/DiffuseColor:1  1  1  1

AddBoxShape
	/Dimensions:4.3  0.001  0.3  	/Mass:0.1
	/Position:0    0.006    2
	/DiffuseColor:0    0    0    1

AddBoxShape
	/Dimensions:4.3  0.001  0.3  	/Mass:0.1
	/Position:0    0.006    -2
	/DiffuseColor:0    0    0    1

AddBoxShape
	/Dimensions:0.3  0.001  4  	/Mass:0.1
	/Position:2    0.006    0
	/DiffuseColor:0    0    0    1

AddBoxShape
	/Dimensions:0.3  0.001  4  	/Mass:0.1
	/Position:-2    0.006    0
	/DiffuseColor:0    0    0    1 

FlushScript  


AddDifferentialDriveEntity  base1
	/Position:0  0.01  2
	/Orientation:0  90  0

AddColorSensorEntity  color1
	/Position:-0.2  0.2  -0.3
	/ParentEntity:base1
	/Orientation:-90    0    0
	/Procedure_SensorNotify:proc_color1

AddColorSensorEntity  color2
	/Position:0  0.2  -0.3
	/ParentEntity:base1
	/Orientation:-90    0    0
	/Procedure_SensorNotify:proc_color2

AddColorSensorEntity  color3
	/Position:0.2  0.2  -0.3
	/ParentEntity:base1
	/Orientation:-90    0    0
	/Procedure_SensorNotify:proc_color3

FlushScript  

procedure  proc_color1

	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_color2

	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("i2c3", sendBytes)
			
end 

procedure  proc_color3

	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("i2c4", sendBytes)
			
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 

I2CDevice    i2c1
	/Address:58
	/ClockRateKhz:100

I2CDevice    i2c2
	/Address:60
	/ClockRateKhz:100

I2CDevice    i2c3
	/Address:62
	/ClockRateKhz:100

I2CDevice    i2c4
	/Address:64
	/ClockRateKhz:100


colorBuff1 = Util.CreateArrayByte(3)
colorBuff2 = Util.CreateArrayByte(3)
colorBuff3 = Util.CreateArrayByte(3)

c1 = 765
c2 = 765
c3 = 765

buff1 = Util.CreateArrayByte(3)

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

procedure proc_read_color
	
	skip_count = 0
	
	while(true)
	{	
		i2c2.Read(colorBuff1)
		i2c3.Read(colorBuff2)
		i2c4.Read(colorBuff3)
			
		c1 = colorBuff1[0] + colorBuff1[1] + colorBuff1[2]
		c2 = colorBuff2[0] + colorBuff2[1] + colorBuff2[2]
		c3 = colorBuff3[0] + colorBuff3[1] + colorBuff3[2]

		if (c1 <= 10)
			c1 = 765

		if (c2 <= 10)
			c2 = 0

		if (c3 <= 10)
			c3 = 765			
		
		if (skip_count == 0)
		{
			if (c1 < 255 && c3 >= 255)
			{
				//turn left
				buff1[0] = 2
				buff1[1] = 130	//130-100
				buff1[2] = 110	//0.1			
				i2c1.Write(buff1)
				
				//print "Turn left -> " + c1 + " / " + c2 + " / " + c3
				
				skip_count = 3
			}
			else 
			{
				if (c1 >= 255 && c3 < 255)
				{
					//turn right
					buff1[0] = 2
					buff1[1] = 70	//70-100
					buff1[2] = 110	//0.1			
					i2c1.Write(buff1)

					//print "Turn right -> " + c1 + " / " + c2 + " / " + c3
					
					skip_count = 3
				}
				else 
				{
					if (c1 >= 255 && c3 >= 255 && c2 >= 255)
					{
						//turn right
						buff1[0] = 2
						buff1[1] = 70	//70-100
						buff1[2] = 110	//0.1			
						i2c1.Write(buff1)
						
						//print "Turn right for find line -> " + c1 + " / " + c2 + " / " + c3

						skip_count = 3
					}
					else
					{
						//go forwards
						buff1[0] = 4
						buff1[1] = 110	//0.1
						buff1[2] = 110	//0.1			
						i2c1.Write(buff1)
						
						//print "Go -> " + c1 + " / " + c2 + " / " + c3

						skip_count = 1
					}
				}
			}
		}

		if (skip_count == 1)
		{
			//go forwards
			buff1[0] = 4
			buff1[1] = 110	//0.1
			buff1[2] = 110	//0.1
			i2c1.Write(buff1)
		}

		if (skip_count > 0)
			skip_count--
			
		wait 300
	}
	
end