#--------------------------------------------------------
# This is a scriptcontext based driver for the EL734
# motor controller. This is part of an ongoing effort to
# expire older drivers and to consolidate on the new
# scriptcontext system.
#
# Scriptchains:
#     Rather then having long script chains many of the
#     intricacies of the EL734 are handled via a command
#     processing state machine. See the docs below for
#     details
#
# copyright: see file COPYRIGHT
#
# Mark Koennecke, February 2011
#--------------------------------------------------------

namespace eval el734 {}

#---------------------------------------------------------
# The EL734 is a a tricky thing. Some special conditions
# apply:
# - On emergency stop an *ES is sent. But only the second
#   response of this kind is valid because there can be
#   spurious *ES on the line even when the emergency stop
#   has been released.
# - If someone fingers the EL734 or after startup it is in
#   local mode. Then two commands have to be sent in order to
#   make it go into remote mode before retrying the command.
# - In some echo modes of the controller it sends a echo
#   of the command. This has to be ignored in order to get at
#   the real problem
#
# In order to deal with all this, el734::command is implemented
# as a state machine which calls another script when a valid
# reponse has actually been found.
# The state of the current command processing
# is saved in a node property comstate. The actual command to send
# is in the property comstring. The script to call if we actually
# have a valid response is stored in the property comresponse
#---------------------------------------------------------------
proc el734::setcommand {command responsescript {motno 0}} {
    sct comresponse $responsescript
    sct comstate start
    sct comstring $command
    sct commotno $motno
    return command
}
#---------------------------------------------------------------
# As implemented now this can go in an endless loop if switching
# to local fails repeatedly. TODO: test if this happens with the
# real device
#---------------------------------------------------------------
proc el734::command {} {
    set state [sct comstate]
    switch $state {
	start {
	    set com [sct comstring]
	    sct send $com
	    sct comstate waitresponse
	}
	waitstart {
	    wait 1
	    sct comstate start
	    return [el734::command]
	}
	waitresponse {
	    set reply [sct result]
	    if {[string first "*ES" $reply] >= 0} {
		set com [sct comstring]
		sct send $com
		sct comstate waitES
		return command
            }
	    if {[string first "?LOC" $reply] >= 0} {
		sct send "RMT 1"
		sct comstate waitrmt
		return command
            }
	    if {[string first "?BSY" $reply] >= 0} {
		set mot [sct commotno]
		if {$mot != 0} {
		    set com [format "S %d" $mot]
		} else {
		    set com "S"
		}
		sct send $com
		sct comstate waitstart
		return command
            }
	    set com [sct comstring]
	    set idx [string first $com $reply]
	    if {[string first $com $reply] >= 0} {
		sct send @@NOSEND@@
		sct comstate waitresponse
		return command
            }
	    set responsescript [sct comresponse]
	    return [eval $responsescript]
        }
	waitES {
	    set reply [sct result]
	    if {[string first "*ES" $reply] >= 0} {
		clientput "Emergency STOP ENGAGED, release to continue"
		error "Emergency Stop ENGAGED"
            }
	    set responsescript [sct comresponse]
	    return [eval $responsescript]
        }
	waitrmt {
	    sct send "ECHO 0"
	    sct comstate  start
	}
    }
    return command
}
#-------------------------------------------------------------------
proc el734::checkerror {} {
    set err(?ADR) "Bad address"
    set err(?CMD) "Bad command"
    set err(?PAR) "Bad parameter"
    set err(?RNG) "Parameter out of range"
    set err(?BSY) "Motor busy"
    set err(*MS) "Bad step"
    set err(*ES) "Emergency stop engaged"

    set reply [string trim [sct result]]
    set errlist [array names err]
    foreach entry $errlist {
	if {[string first $entry $reply] >= 0} {
	    error $err($entry)
	}
    }
    return $reply
}
#========================== Position ===============================
proc el734::readpos {num} {
    set com [format "u %d" $num]
    return [el734::setcommand $com el734::posresponse]
}
#-------------------------------------------------------------------
proc el734::posresponse {} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	sct update $reply
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#-------------------------------------------------------------------
proc el734::setpos {name num} {
    set newpos [sct target]
    set com [format "p %d %f" $num $newpos]
    hupdate /sics/${name}/status run
    hupdate /sics/${name}/oredmsr 3
    hupdate /sics/${name}/runfault 0
    hupdate /sics/${name}/posfault 0
    return [el734::setcommand $com "el734::setposresponse $name"]
}
#-------------------------------------------------------------------
proc el734::setposresponse {name} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	[sct controller] queue /sics/${name}/status progress read
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#===================== Limits =====================================
proc el734::getlim {name num} {
    set com [format "H %d" $num]
    return [el734::setcommand $com "el734::limresponse $name"]
}
#-----------------------------------------------------------------
proc el734::limresponse {name} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	stscan $reply "%f %f" low high
	hupdate /sics/${name}/hardlowerlim $low
	hupdate /sics/${name}/hardupperlim $high
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#------------------------------------------------------------------
proc el734::setlim {controller name num low high} {
    set com [format "H %d %f %f" $num $low $high]
    $controller send $com
    $controller queue /sics/${name}/hardlowerlim progress read
    wait 1
    return Done
}
#======================== status ================================
proc el734::decodemsr {name msr} {
    set oredata(0x02) idle:none
    set oredata(0x10) error:lowlim
    set oredata(0x20) error:hilim
    set oredata(0x80)  posfault:runfault
    set oredata(0x200) posfault:posfault
    set oredata(0x1000) "error:air cushion"
    set oredata(0x40) "error:bad step"
    set oredata(0x100) error:positionfault
    set oredata(0x400) error:positionfault

    set msrdata(0x20) hilim
    set msrdata(0x10) lowlim
    set msrdata(0x1000) "air cushion"
    set msrdata(0x40) "Bad step"
    set msrdata(0x100) posfault
    set msrdata(0x400) posfault

    set oredmsr [hval /sics/${name}/oredmsr]
    if {$msr == 0} {
#-------- FINISHED
	set pos [hval /sics/${name}/posfault]
	set run [hval /sics/${name}/runfault]
	if {$pos > 0 || $run > 0} {
	    return posfault
        }

	set orlist [array names oredata]
	foreach code $orlist {
	    if {$oredmsr & $code} {
		set l [split $oredata($code) :]
		set txt [lindex $l 1]
		set ret [lindex $l 0]
		hupdate /sics/${name}/lasterror $txt
		if {[string compare $ret error] == 0} {
		    clientput "ERROR: $txt on motor $name"
                }
		return $ret
            }
        }
	if {$oredmsr == 0} {
	    return idle
	}
    } else {
#------------ Still Running.........
	set msrlist [array names msrdata]
	foreach code $msrlist {
	    if {$msr & $code} {
		clientput "ERROR: $msrdata($code) on motor $name"
		return error
            }
        }
	if {$msr & 0x80} {
	    set val [hval /sics/${name}/runfault]
	    incr val
	    hupdate /sics/${name}/runfault $val
        }
	if {$msr & 0x200} {
	    set val [hval /sics/${name}/posfault]
	    incr val
	    hupdate /sics/${name}/posfault $val
        }

	hupdate /sics/${name}/oredmsr [expr $oredmsr | $msr]
	return run
    }
}
#----------------------------------------------------------------
proc el734::readstatus {num name} {
    set com [format "msr %d" $num]
    return [el734::setcommand $com "el734::statresponse $name $num"]
}
#----------------------------------------------------------------
proc el734::statresponse {name num} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	stscan $reply "%d" msr
	set status [el734::decodemsr $name $msr]
	sct update $status
	switch $status {
	    run {
		set con [sct controller]
		$con queue /sics/${name}/hardposition progress read
		$con queue /sics/${name}/status progress read
	    }
	    idle {
		set com [format "u %d" $num]
		return [el734::setcommand $com "el734::posstat $name" ]
            }
        }
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#----------------------------------------------------------------
proc el734::posstat {name} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	hupdate /sics/${name}/hardposition $reply
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#========================== Halt =================================
proc el734::halt {controller no} {
    set com [format "S %d" $no]
    $controller send $com
    return Done
}
#========================= Speed ================================
proc el734::readspeed {num} {
    set com [format "J %d" $num]
    return [el734::setcommand $com el734::speedresponse]
}
#-------------------------------------------------------------------
proc el734::speedresponse {} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	sct update $reply
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#-------------------------------------------------------------------
proc el734::setspeed {name num} {
    set newpos [sct target]
    set com [format "J %d %d" $num $newpos]
    return [el734::setcommand $com "el734::setspeedresponse $name $num"]
}
#-------------------------------------------------------------------
proc el734::setspeedresponse {name num} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	return [el734::readspeed $num]
    } else {
	clientput $reply
	return idle
    }
}
#========================= refnull ================================
proc el734::readref {num} {
    set com [format "V %d" $num]
    return [el734::setcommand $com el734::refresponse]
}
#-------------------------------------------------------------------
proc el734::refresponse {} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	sct update $reply
	return idle
    } else {
	clientput $reply
	return idle
    }
}
#-------------------------------------------------------------------
proc el734::setref {name num} {
    set newpos [sct target]
    set com [format "V %d %d" $num $newpos]
    return [el734::setcommand $com "el734::setrefresponse $name $num"]
}
#-------------------------------------------------------------------
proc el734::setrefresponse {name num} {
    set stat [catch {checkerror} reply]
    if {$stat == 0} {
	return [el734::readref $num]
    } else {
	clientput $reply
	return idle
    }
}
#============================= SS =================================
proc el734::readss {num} {
    set com [format "SS %d" $num]
    sct send $com
    return ssread
}
#-----------------------------------------------------------------
proc el734::ssread {} {
    sct update [sct result]
    return idle
}
#======================== setpos ================================
proc el734::forcepos {controller name num newpos} {
    set com [format "U %d %f" $num $newpos]
    $controller send $com
    $controller queue /sics/${name}/hardposition progress read
    wait 1
    return Done
}
#======================= refrun ==================================
proc el734::refrun {controller name num} {
    clientput "Starting reference run"
    $controller send [format "R %d" $num]
    $controller queue /sics/${name}/ss progress read
    while {1} {
	wait 2
	set ss [hval /sics/${name}/ss]
	if { [string first ?BSY $ss] < 0} {
		break
	}
	set rupt [getint]
	if { [string compare $rupt continue] != 0} {
	    error "Refererence run interrupted"
	}
	$controller queue /sics/${name}/ss progress read
    }
    $controller queue /sics/${name}/hardposition progress read
    wait 2
    return "Reference run Finished"
}
#================================================================
proc el734::reset {name} {
    set x [hval /sics/${name}/hardlowerlim]
    hupdate /sics/${name}/softlowerlim $x
    set x [hval /sics/${name}/hardupperlim]
    hupdate /sics/${name}/softupperlim $x
    hupdate /sics/${name}/softzero 0
    hupdate /sics/${name}/fixed -1
}
#========================= Make ==================================
proc el734::make {name no controller} {
    MakeSecMotor $name

    hfactory /sics/${name}/oredmsr plain internal int
    hfactory /sics/${name}/runfault plain internal int
    hfactory /sics/${name}/posfault plain internal int
    hfactory /sics/${name}/lasterror plain internal text

    hsetprop /sics/${name}/hardposition read el734::readpos $no
    hsetprop /sics/${name}/hardposition command el734::command

    hsetprop /sics/${name}/hardposition write el734::setpos $name $no
    hsetprop /sics/${name}/hardposition command el734::command
    $controller write /sics/${name}/hardposition

    hsetprop /sics/${name}/hardlowerlim read el734::getlim $name $no
    hsetprop /sics/${name}/hardlowerlim command el734::command
    $controller poll /sics/${name}/hardlowerlim 120

    hsetprop /sics/${name}/status read el734::readstatus $no $name
    hsetprop /sics/${name}/status command el734::command
    $controller poll /sics/${name}/status 40

    hfactory /sics/${name}/speed plain user int
    hsetprop /sics/${name}/speed read el734::readspeed $no
    hsetprop /sics/${name}/speed command el734::command
    $controller poll /sics/${name}/speed 120

    hsetprop /sics/${name}/speed write el734::setspeed $name $no
    hsetprop /sics/${name}/speed command el734::command
    $controller write /sics/${name}/speed

    $name makescriptfunc halt "el734::halt $controller $no" user
    $name makescriptfunc reset "el734::reset $name" user

    $name makescriptfunc sethardlim "el734::setlim $controller $name $no" mugger
    hfactory /sics/${name}/sethardlim/low plain mugger float
    hfactory /sics/${name}/sethardlim/high plain mugger float

    hfactory /sics/${name}/motno plain internal int
    hupdate /sics/${name}/motno $no

}
#-------------------------------------------------------------------------------
proc el734::addrefstuff {name no controller} {
    hfactory /sics/${name}/refnull plain user int
    hsetprop /sics/${name}/refnull read el734::readref $no
    hsetprop /sics/${name}/refnull command el734::command
    $controller poll /sics/${name}/refnull 300

    hsetprop /sics/${name}/refnull write el734::setref $name $no
    hsetprop /sics/${name}/refnull command el734::command
    $controller write /sics/${name}/refnull

    hfactory /sics/${name}/ss plain internal text
    hsetprop /sics/${name}/ss read el734::readss $no
    hsetprop /sics/${name}/ss ssread el734::ssread
    $controller poll /sics/${name}/ss 300

    $name makescriptfunc refrun "el734::refrun $controller $name $no" user

}