#---------------------------------------------------------------
# These are the scripts for the delta-tau PMAC motor 
# controller.
#
# !!!!!!!!! Script Chains !!!!!!!!!!!
# -- For reading parameters:
#      sendpmacread code -- pmacreadreply
# -- For setting standard parameters
#      sendpmacwrite code -- pmacreadreply
# -- For reading limits
#      sendpmaclim -- readpmaclim
# -- For reading the status
#      pmacsendaxer --- pmacrcvaxerr -- pmacrcvpos -- pmacrcvstat
#    This means we check for an axis error first, then update the position,
#    then check the axis status itself.   
# -- For setting the position
#      pmacsendhardpos -- pmacrcvhardpos -- pmacrcvhardax
#    This means, we send the positioning command, read the reply and read the 
#    axisstatus until the axis has started 
#
# copyright: see file COPYRIGHT
#
# Mark Koennecke, December 2008, March 2009
#---------------------------------------------------------------
proc translatePMACError {key} {
    set pmacerr(ERR001) "Command not allowed while executing"
    set pmacerr(ERR002) "Password error"
    set pmacerr(ERR003) "Unrecognized command"
    set pmacerr(ERR004) "Illegal character"
    set pmacerr(ERR005) "Command not allowed"
    set pmacerr(ERR006) "No room in buffer for command"
    set pmacerr(ERR007) "Buffer already in use"
    set pmacerr(ERR008) "MACRO auxiliary communication error"
    set pmacerr(ERR009) "Bad program in MCU"
    set pmacerr(ERR010) "Both HW limits set"
    set pmacerr(ERR011) "Previous move did not complete"
    set pmacerr(ERR012) "A motor is open looped"
    set pmacerr(ERR013) "A motor is not activated"
    set pmacerr(ERR014) "No motors"
    set pmacerr(ERR015) "No valid program in MCU"
    set pmacerr(ERR016) "Bad program in MCU"
    set pmacerr(ERR017) "Trying to resume after H or Q"
    set pmacerr(ERR018) "Invalid operation during move"
    set pmacerr(ERR019) "Illegal position change command during move"
    return $pmacerr($key)
}
#------------------------------------------------------------------
proc translateAxisError {key} {
    switch [string trim $key] {
	0 {return "no error"}
	1 { return "limit violation"}
	2 -
	3 -
	4 { return "jog error"}
	5 {return "command not allowed"}
	6 {return "watchdog triggered"}
	7 {return "current limit reached"}
	8 -
	9 {return "Air cushion error"}
	10 {return "MCU lim reached"}
	11 {return "following error triggered"}
	12 {return "EMERGENCY STOP ACTIVATED"}
	13 {return "Driver electronics error"}
	default { return "Unknown axis error $key"}
    }
}
#---------------------------------------------------------------------
proc evaluateAxisStatus {key} {
#----- Tcl does not like negative numbers as keys. 
    if {$key < 0} {
	set key [expr 50 + abs($key)]
    }
    switch $key {
	0 -
	14 {return idle}
	1 -
        2 -
        3 -
        4 -
        5 -
        6 -
        7 -
        8 -
        9 -
        10 -
        11 {return run}
        56 {error "Controller aborted"} 
        55 {error "Axis is deactivated"}
	54 {error "emergency stop activated, please release"}
	53 {error "Axis inhibited"}
	51 -
	52 {error "Incoming command is blocked"}
    }
}
#-----------------------------------------------------------------------
proc checkpmacresult {} {
    set data [sct result]
    if {[string first ASCERR $data] >= 0} {
	error $data
    }
    if {[string first ERR $data] >= 0} {
	error [translatePMACError $data]
    }
    return [string trim $data]
}
#------------------------------------------------------------------------
proc sendpmacread {code} {
    sct send $code
    return pmacreadreply
}
#------------------------------------------------------------------------
proc pmacreadreply {} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	sct geterror $data
    } else {
	sct update $data
    }
    return idle
}
#----------------------------------------------------------------------
proc sendpmaclim {code} {
    sct send $code
    return pmacreadlim
}
#-----------------------------------------------------------------------
proc pmacreadlim {motname} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	sct geterror $data
    } else {
	set scale [hval /sics/${motname}/scale_factor]
	sct update [expr $data * $scale]
    }
    return idle
}
#------------------------------------------------------------------------
proc sendpmacwrite {code} {
    set value [sct target]
    sct send "$code=$value"
    return pmacwritereply
}
#------------------------------------------------------------------------
proc pmacwritereply {} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	sct geterror $data
	sct print "ERROR: $data"
    } else {
	set con [sct controller]
	$con queue [sct] read read
    }
    return idle
}
#-------------------------------------------------------------------------
proc configurePMACPar {name par code sct} {
    set path /sics/$name/$par
    hsetprop $path read "sendpmacread $code"
    hsetprop $path pmacreadreply pmacreadreply
    $sct poll $path 30
    hsetprop $path write "sendpmacwrite $code"
    hsetprop $path pmacwritereply pmacwritereply
    $sct write $path
}
#-------------------------------------------------------------------------
proc makePMACPar {name par code sct priv} {
    set path /sics/$name/$par
    hfactory $path plain $priv float
    configurePMACPar $name $par $code $sct
}
#========================== status functions =============================
proc pmacsendaxerr {num} {
    sct send "P${num}01"
    return rcvaxerr
}
#------------------------------------------------------------------------
proc pmacrcvaxerr {motname num} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	clientput "ERROR: $data"
	sct update error
	sct geterror $data
	return idle
    } 
    hupdate /sics/$motname/axiserror $data
    if {$data != 0 } {
	set err [translateAxisError $data]
	if {[string first following $err] >= 0} {
	    clientput "WARNING: $err"
	    sct update poserror
	} else {
	    clientput "ERROR: $err"
	    sct update error
	}
	return idle
    }
    hupdate /sics/$motname/axiserror $data
    sct send "Q${num}10"
    return rcvpos
}
#------------------------------------------------------------------------
proc pmacrcvpos {motname num} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	clientput "ERROR: $data"
	sct geterror $data
	sct update error
	return idle
    } 
    hupdate /sics/$motname/hardposition $data
    sct send "P${num}00"
    return rcvstat
}
#------------------------------------------------------------------------
proc pmacrcvstat {motname num sct} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	clientput "ERROR: $data"
	sct update error
	return idle
    } 
    set status [catch {evaluateAxisStatus $data} msg]
    if {$status != 0} {
	sct update error
    } else {
	sct update $msg
	switch $msg {
	    idle {
                # force an update of the motor position
		$sct queue /sics/$motname/hardposition progress read
	    }
	    run {
		# force an update of ourselves, while running
		$sct queue /sics/$motname/status progress read
	    }
	}
    }
    return idle
}
#-------------------------------------------------------------------------
proc configurePMACStatus {motname num sct} {
    set path /sics/$motname/status
    hsetprop $path read "pmacsendaxerr $num"
    hsetprop $path rcvaxerr "pmacrcvaxerr $motname $num"
    hsetprop $path rcvpos "pmacrcvpos $motname $num"
    hsetprop $path rcvstat "pmacrcvstat $motname $num $sct"
    $sct poll $path 60
}
#======================= setting hard position ===========================
proc pmacsendhardpos {motname num} {
    hupdate /sics/$motname/status run
    set value [sct target]
    sct send [format "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1" $num $num $value $num]
    return rcvhardpos
}
#-------------------------------------------------------------------------
proc pmacrcvhardpos {num} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	clientput "ERROR: $data"
	sct seterror $data
	return idle
    } 
    sct send "P${num}00"
    return rcvhardax
}
#------------------------------------------------------------------------
proc pmacrcvhardax {motname num sct} {
    set status [catch {checkpmacresult} data]
    if {$status != 0} {
	clientput "ERROR: $data"
	sct seterror $data
	return idle
    } 
    set status [catch {evaluateAxisStatus $data} msg]
    if {$status != 0} {
	clientput "ERROR: $msg"
	sct seterror $msg
	return idle
    }
    switch $msg {
	idle {
	    sct send "P${num}00"
	    return rcvhardax
	}
	run {
	    $sct queue /sics/$motname/status progress read 
	    return idle
	}
    }
}
#------------------------------------------------------------------------
proc configurePMAChardwrite {motname num sct} {
    set path /sics/$motname/hardposition
    hsetprop $path write "pmacsendhardpos $motname $num"
    hsetprop $path rcvhardpos "pmacrcvhardpos $num"
    hsetprop $path rcvhardax "pmacrcvhardax $motname $num $sct"
}
#======================= Halt =============================================
proc pmacHalt {sct num} {
    $sct send "M${num}=8" halt
    return OK
}
#==================== Reference Run =======================================
proc pmacrefrun {motorname sct num} {
    set path /sics/${motorname}/status
    $sct send "M${num}=9"
    hupdate /sics/${motorname}/status run
    set motstat run
    wait 3
    while {[string compare $motstat run] == 0} {
	$sct queue $path progress read
	wait 1
	set motstat [string trim [hval $path]]
    }
    return "Done"
}
#--------------------------------------------------------------------------
proc MakeDeltaTau {name sct num} {
    MakeSecMotor $name
    hsetprop /sics/${name}/hardupperlim read "sendpmaclim I${num}13"
    hsetprop /sics/${name}/hardupperlim pmacreadlim "pmacreadlim $name"
    $sct poll /sics/${name}/hardupperlim 180
    hsetprop /sics/${name}/hardlowerlim read "sendpmaclim I${num}14"
    hsetprop /sics/${name}/hardlowerlim pmacreadlim "pmacreadlim $name"
    $sct poll /sics/${name}/hardlowerlim 180

#    configurePMACPar $name hardlowerlim "Q${num}09" $sct
#    configurePMACPar $name hardupperlim "Q${num}08" $sct

    configurePMACPar $name hardposition "Q${num}10" $sct
    configurePMAChardwrite $name $num $sct
    hfactory /sics/$name/numinmcu plain internal int
    hupdate /sics/$name/numinmcu ${num}
    makePMACPar $name scale_factor   "Q${num}00" $sct mugger
    makePMACPar $name maxspeed       "Q${num}03" $sct mugger
    makePMACPar $name commandspeed   "Q${num}04" $sct mugger
    makePMACPar $name maxaccel       "Q${num}05" $sct mugger
    makePMACPar $name commandedaccel "Q${num}06" $sct mugger
    makePMACPar $name offset         "Q${num}07" $sct mugger
    makePMACPar $name axisstatus     "P${num}00" $sct internal
    makePMACPar $name axiserror      "P${num}01" $sct internal
    makePMACPar $name poshwlimitactive "M${num}21" $sct internal
    makePMACPar $name neghwlimitactive "M${num}22" $sct internal
    makePMACPar $name liftaircushion   "M${num}96" $sct internal
    configurePMACStatus $name $num $sct
    $name makescriptfunc halt "pmacHalt $sct $num" user
    $name makescriptfunc refrun "pmacrefrun $name $sct $num" user
    set parlist [list scale_factor  hardposition  maxspeed \
		 commandspeed maxaccel offset axisstatus axiserror status poshwlimitactive \
		  neghwlimitactive liftaircushion hardlowerlim hardupperlim]
    $sct send [format "M%2.2d14=0" $num]
    foreach par $parlist {
	$sct queue /sics/$name/$par progress read
    } 
}