global [fd_threshold 
	bk_threshold 
	lt_threshold 
	rt_threshold 
	honk_threshold 
	idle_threshold
	FrontCMD 		; CMD to exe when front sensor is pressed
	RearCMD			; CMD to exe when rear sensor is pressed
	TurnDuration		; Defines the turn duration (sec) 
	RunDuration		; Defines the run (fd/bk) duration (sec)
	]


to Start
	let [CmdCount	0
		CmdMax	20
		hold	0]
	
	setdp 0
	

	setfd_threshold 45
	setbk_threshold 195
	setlt_threshold 350
	setrt_threshold 520 
;	sethonk_threshold 530

	setFrontCMD 	2	; Default -> move backward 
	setRearCMD	1	; Default -> move forward

	setTurnDuration 1500
	setRunDuration 	1500
	
	
	loop [	
		; U P L O A D    &   D O W N L O A D   C O M M A N D S

		if newir?
		[ if ir = 85
	  	  [
		    ; mark end of command list before upload
		    setdp :CmdCount
		    record 0
		    
		    UploadData
		    
		    beep
		    setdp :CmdCount
		  ]
		  if ir = 255
		  [

		    ; ReceiveData returns # of cmd received
		    make "hold ReceiveData
		    if :hold > 0 [   make "CmdCount :hold]
		    setdp :CmdCount
		    
		    beep
		  ]
		]
			
		
		;;======================================================================
		;  P L A Y B A C K
		
		; hit sensor2 to start playback / hit again to stop
		if sensor2 < 500
		[ resett
		  waituntil [sensor2 > 500]
		  
		  setdp :CmdCount

	  	; mark end of command list before playback
	    	  record 0

		  ; play once
		  ifelse timer < 1000
		  [
		  	note a 2 note b 2 note c2 2
		  

		  	if ExecuteStoredCMD = 0 [beep]
		  ]
		  ; play indefinitely
		  [
		  	note c2 2 wait 2 note c2 2
		  	
		  	ExecuteStoredCMDindef
		  ]
		  
		  ;with out this delay the car will stop and start again immediately
		  beep
		  wait 5
		  
		  setdp :CmdCount
		  
	
		]
	



		;;======================================================================
		;  R E C O R D   
		ifelse :CmdCount < ( :CmdMax + 1)
		[
			if RecordData > 0
			[	make "CmdCount :CmdCount + 1
			]
		]
		[	; if reached the max cmd -> play sound	
			if :CmdCount = (:CmdMax + 1)
			[	record 0	
				repeat 5 [ note c 2 note g 2]
				make "CmdCount :CmdCount + 1
			]
		]
	     ]

end


to RecordData
	let [Key 0]

	make "Key KeyPressed
	if not (:Key = 0)
	[
		record :Key
		ExecuteCMD :Key
	]
	
	output :Key
	
end

 

to KeyPressed
let [keyval 0 DoItButton 0]
let [	Up 	fd_threshold 
	Down	bk_threshold 
	Left	lt_threshold 
	Right	rt_threshold 
	Honk	honk_threshold 
    ]


loop [	

	make "keyval GetStableReadingOfSensor1
	make "DoItButton sensor2

	if :keyval > 900 [ output 0] 
	
;	if :keyval > :Down 	[  output 2]
;	if :keyval > :Right 	[  output 4]
;	if :keyval < :Up 	[  output 1]
;	if :keyval < :Left 	[  output 3]

	if :keyval < :Up	[  output 1]
	if :keyval < :Down	[  output 2]
	if :keyval < :Left	[  output 3]
	if :keyval < :Right	[  output 4]	
;	if :keyval < :Honk	[  output 5]	
	
	output 0
]
 
end



to GetStableReadingOfSensor1
let [   PrevVal 0
	CurVal 0
	ThreshHold 5]
	
    loop [
	    make :CurVal sensor1
	    if abs (:CurVal - :PrevVal) < :ThreshHold
	    [	if (not (:PrevVal  = 0))
	    	[	output :CurVal
	    	]
	    ]
	    make "PrevVal :CurVal
	    wait 0.1
	 ]
end






to UploadData 
let [	UploadHeader 	170
	UploadData 	0 ]

	setdp 0
	send :UploadHeader
	loop [ 
		make "UploadData recall
		send :UploadData
		if :UploadData = 0 [ stop]
		
	     ]
end


to ReceiveData
	let [count 0 InByte 0]
	
	setdp 0
	resett

	loop [
		if timer > 1000 [output :count]  
		if newir? [
			make "InByte ir

			; InByte between 11-15 -> defines the CMD when front sensor is pressed
			if (:InByte > 10) and (:InByte < 20)
			[	setFrontCMD (:InByte - 10)
			]

			; InByte between 21-25 -> defines the CMD when rear sensor is pressed			
			if (:InByte > 20) and (:InByte < 30)
			[	setRearCMD (:InByte - 20)
			]
			
			; InByte between 31-39 -> define secods that the car should turn
			if (:InByte > 30) and (:InByte < 40)
			[	setTurnDuration ((:InByte - 30) * 400)
			]
			
			; InByte between 41-49 -> define secods that the car should move fd/bk
			if (:InByte > 40) and (:InByte < 50)
			[	setRunDuration ((:InByte - 40) * 400)
			]
			
			
			; InByte < 10 is a car command
			if :InByte < 10
			[	record :InByte
				make "count :count + 1
			]
			
			resett 
			
			]
	     ]
end



to ExecuteStoredCMDindef
	loop [if ExecuteStoredCMD = 0 [stop]]
end


to ExecuteStoredCMD
   let [CMD 0]

   setdp 0
   loop [
	if sensor2 < 500 [output 0]
	make "CMD recall
	ifelse :CMD = 0 
	[ output 1 ]
	[ ExecuteCMD :CMD ]
   ]
end


to ExecuteCMD :CMD

	if :CMD = 1 [ goFD ]
	if :CMD = 2 [ goBK ]
	if :CMD = 3 [ goLT ]
	if :CMD = 4 [ goRT ]
	if :CMD = 5 [ Honk ]

	MoveCarefully :CMD
	
	ab, off
	
	

end


to MoveCarefully :CMD
	let [SensorCMD 0]
	let [CMDDuration 1500]
	
	ifelse (:CMD = 1) or (:CMD = 2) [make "CMDDuration RunDuration]
		[	make "CMDDuration TurnDuration]

	resett
	loop
	[	if timer > :CMDduration [ stop ]
		
		if sensor3 < 512
		[ ifelse :CMD = 2 
			[ make "SensorCMD RearCMD
			] 
			[ make "SensorCMD FrontCMD
			] 
			make "CMD :SensorCMD
		
			if :SensorCMD = 1 [ goFD ]
			if :SensorCMD = 2 [ goBK ]
			if :SensorCMD = 3 [ goLT ]
			if :SensorCMD = 4 [ goRT ]
			if :SensorCMD = 5 [ Honk ]
			
			; without this wait, multiple instances of the same event will be executed.
			wait 3
			resett
		]
	]
end




to goFD
	ab, thisway on
end

to goBK
	ab, thatway on
end

to goLT
	a, thatway
	b, thisway
	ab, on
end

to goRT
	a, thisway
	b, thatway
	ab, on
end

to Honk
	note f 1 wait 0.5 note f 1
end