#------------------------------------------------------------------------------
# Regression tests fo a motor. It is assumed that the motors name is 
# brumm and that it has been initialized with the regress motor 
# driver. Moreover, this has to be loaded after tcltest.tcl, testutil.tcl
# and sicstcldebug.tcl
#
# The regression motor has various errortypes which can be simulated:
#  0   = none
#  1   = failed start
#  2   = position fault
#  3   = hardware failure
#  4   = off position, without explicit position fault
#  5   = failed read
#  6   = keep running (for testing interupting)
# Moreover there is a recover flag which causes the motor to recover when it is
# 1
# 
# copyright: see file COPYRIGHT
#
# Mark Koennecke, July 2006
#
#------------------------------------------------------------------------------
puts stdout "Testing motor code"

test motorpar-1.0 {Test sll} -body {
  testPar "brumm softlowerlim" -175 User } -result OK

test motorpar-1.1 {Test slu} -body {
  testPar "brumm softupperlim" 175 User } -result OK

test motorpar-1.2 {Test ss} -body {
  testPar "brumm softzero" 5 User } -result OK
 
test motorpar-1.3 {Test interrupt} -body {
  testPar "brumm interruptmode" 2 Mugger } -result OK

test motorpar-1.4 {Test accesscode} -body {
  testPar "brumm accesscode" 3 Mugger } -result OK
 
test motorpar-1.5 {Test precision} -body {
  testPar "brumm precision" .5 Mugger } -result OK

test motorpar-1.6 {Test fail} -body {
  testPar "brumm failafter" 5 Mugger } -result OK

test motorpar-1.7 {Test retry} -body {
  testPar "brumm maxretry" 5 Mugger } -result OK

test motorpar-1.8 {Test ignorefault} -body {
  testPar "brumm ignorefault" 1 Mugger } -result OK
 
test motorpar-1.9 {Test movecount} -body {
  testPar "brumm movecount" 12 Mugger } -result OK

#-------- This test always fails because the regression motor driver 
# handles hard limits
#---------------------------------------------------------------
# test motorpar-1.10 {Test hardupper} -body {
#   testPar "brumm hardupperlim" 180 Mugger} -result OK

# test motorpar-1.11 {Test hardlower} -body {
#   testPar "brumm hardlowerlim" -180 Mugger} -result OK

brumm recover 0
brumm errortype 0
test motor-1.0 {Test Normal Driving} -body {
	testDrive brumm 10 User } -result OK
	
brumm errortype 6
test motor-1.1 {Test Interrupting} -body {
	testDriveInterrupt brumm 0 } -result OK
	
brumm errortype 1
config rights User User

test motor-1.2 {Test Start Failure} -body {
	set ans [drive brumm 20.3]
	if { [string first "Failed to start motor" $ans] < 0} {
		error "Failed to trigger motor start failure: $ans"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from start failure"
	}
	return OK
} -result OK	

brumm recover 1
test motor-1.3 {Test Recover from start problem} -body {
	set ans [drive brumm 20.3]
	if { [string first "Failed to start motor" $ans] < 0} {
		error "Failed to trigger motor start failure"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from start failure"
	}
	compareValue [SICSValue brumm] 20.3
} -result OK

brumm recover 0
brumm errortype 2

test motor-1.4 {Test Position Failure} -body {
	set ans [drive brumm -20.3]
	if { [string first "Position not reached" $ans] < 0} {
		error "Failed to trigger motor position fault: $ans"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from position failure"
	}
	set ans [SICSValue brumm]
	if {abs($ans - -20.3) < .01} {
		error "Motor positioned OK inspite of position fault"
	}
	return OK
} -result OK	
			
brumm recover 1

test motor-1.6 {Test Position Failure Recover} -body {
	set ans [drive brumm 20.3]
	if { [string first "Position not reached" $ans] < 0} {
		error "Failed to trigger motor position fault: $ans"
	}
	if { [string first "restarting" $ans] < 0} {
		error "Restarting message not received"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from position failure"
	}
	compareValue [SICSValue brumm] 20.3
	return OK
} -result OK	

brumm errortype 3
brumm recover 0
test motor-1.7 {Test Hardware Failure} -body {
	set ans [drive brumm 20.3]
	if { [string first "Hardware is mad" $ans] < 0} {
		error "Failed to trigger motor hardware fault: $ans"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from position failure"
	}
	set ans [SICSValue brumm]
	if {abs($ans - 20.3) < .01} {
		error "Motor positioned OK inspite of hardware fault"
	}
	return OK
} -result OK	
			
brumm recover 1

test motor-1.8 {Test Hardware Failure Recover} -body {
	set ans [drive brumm 20.3]
	if { [string first "Hardware is mad" $ans] < 0} {
		error "Failed to trigger motor hardware fault: $ans"
	}
	if { [string first "restarting" $ans] < 0} {
		error "Restarting message not received"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return from hardware failure"
	}
	compareValue [SICSValue brumm] 20.3
	return OK
} -result OK	

brumm errortype 4

test motor-1.9 {Test Consistent Mispositioning} -body {
	set ans [drive brumm -20.3]
	if { [string first "off position" $ans] < 0} {
		error "Failed to trigger motor off position"
	}
	if { [string first "restarting" $ans] < 0} {
		error "Restarting message not received"
	}
	if { [string first "aborting" $ans] < 0} {
		error "Aborting message not received"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return after consistent position problem"
	}
	set ans [SICSValue brumm]
	if {abs($ans - -20.3) < .01} {
		error "Motor positioned OK inspite of mispositioning"
	}
	return OK
} -result OK	

brumm errortype 0 
drive brumm 27.
			
brumm errortype 5
brumm recover 0
			
test motor-1.10 {Failed read} -body {
	set ans [brumm]
	if { [string first "Failed to read" $ans] < 0} {
		error "Failed to trigger motor failed read"
	}
	if { [string first "Error obtaining position" $ans] < 0} {
		error "Failed to abort reading"
	}
	if { [string first "cannot fix" $ans] < 0} {
		error "Missing cannot fix message"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return after failed read"
	}
	return OK
} -result OK	
brumm  recover 1

test motor-1.11 {Failed read recover} -body {
	set ans [brumm]
	if { [string first "Failed to read" $ans] < 0} {
		error "Failed to trigger motor failed read"
	}
	if { [string first "brumm =" $ans] < 0} {
		error "Motor did not return value after fixing failed read"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return after failed read"
	}
	return OK
} -result OK	

brumm errortype 0
drive brumm 27.
brumm recover 0
brumm errortype 3

test motor-1.12 {Motor Alarm} -body {
	drive brumm -27
	drive brumm -27
	drive brumm -27
	drive brumm -27
	set ans [drive brumm -27]
	if { [string first "MOTOR ALARM" $ans] < 0} {
		error "Motor did not stop with Alarm"
	}
	set ans [status]
	if { [string first "Eager" $ans] < 0} {
		error "Motor did not return after Alarm"
	}
	brumm errortype 0
	set ans [drive brumm  -27]
	if { [string first "sucessfully" $ans] < 0} {
		error "Motor did not recover after Alarm"
	}
	return OK
} -result OK
		
brumm errortype 0
config rights Mugger Mugger

test motor-1.13 {Motor Sign} -body {
	brumm sign 1
    drive brumm 27.
	set old [SICSValue brumm]
	brumm sign -1 
	set newVal [SICSValue brumm]
	set br [brumm sign]
	brumm sign 1
	return [compareValue [expr $old * -1] $newVal]
} -result OK	

brumm sign 1.

test motor-1.14 {Motor Recover} -body {
	brumm sign -1.
	brumm softzero 5
	set data [brumm list]
	backup hugo.bck
	recover hugo.bck
	set newData [brumm list]
	if {[string compare $data $newData] != 0} {
		backup hugo2.bck
		error "Recovery failed: look at diff between hugo.bck and hugo2.bck"
	}
	brumm sign 1
	brumm softzero 0
	set data [brumm list]
	backup hugo.bck
	recover hugo.bck
	set newData [brumm list]
	if {[string compare $data $newData] != 0} {
		backup hugo2.bck
		error "Recovery failed: look at diff between hugo.bck and hugo2.bck"
	}
	return OK	
} -result OK

config rights Spy Spy
test motorpar-1.15 {Test sign setting} -body {
  testPar "brumm sign" -1 Mugger } -result OK