#--------------------------------------------------------------
# This is a new style driver for the Astrium chopper systems in
# the new sicsobj/scriptcontext based system. Please note that
# actual implementations may differ in the number of choppers 
# and the address of the chopper on the network.
#
# copyright: see file COPYRIGHT
#
# SCRIPT CHAINS:
# - reading parameters:
#    astchopread - readastriumchopperpar - readastriumchopperpar - ...
# - writing
#    astchopwrite - astchopwritereply 
#
# Another remark:
#  In order for chosta to work properly, the chopperparlist and 
#  chopperlonglist must be aligned.
#
# Mark Koennecke, February 2009
#
# If something goes wrong with this, the following things ought 
# to be checked:
# - Is the standard Tcl scan command been properly renamed to stscan?
# - Is a communication possible with the chopper via telnet?
#   This may not be the case because of other SICS servers blocking 
#   things or the old driver being active and capturing the terminal
#   server port in SerPortServer. Scriptcontext then fails silently.
#   But may be we will fix the latter.  
# - The other thing which happens is that the parameter list of  
#   the chopper differs in little ways between instances.
#
# Mark Koennecke, April 2009
#--------------------------------------------------------------
MakeSICSObj choco AstriumChopper
#-------------------------------------------------------------
proc astriumchopperputerror {txt} {
    global choppers chopperparlist
    foreach chopper $choppers {
	foreach par $chopperparlist {
	    set path /sics/choco/${chopper}/${par}
	    hsetprop $path geterror $txt
        }
    }
}
#--------------------------------------------------------------
# Paramamters look like: name value, entries for parameters are 
# separated by ;
#---------------------------------------------------------------
proc astriumsplitreply {chopper reply} {
    set parlist [split [string trim $reply] ";"]
    foreach par $parlist {
	catch {stscan $par "%s %s" token val} count
	if {[string first ERROR $count] < 0 && $count == 2} {
	    set val [string trim $val]
	    set token [string trim $token]
	    catch {hupdate /sics/choco/${chopper}/${token} $val}
	    catch {hdelprop /sics/choco/${chopper}/${token} geterror}
	} else {
#-------- special fix for dphas and averl
	    if {[string first dphas $par] >= 0} {
		set val [string range $par 5 end]
		if {$val > 360} {
		    set val [expr $val -360.]
		}
		hupdate /sics/choco/${chopper}/dphas $val 
		hdelprop /sics/choco/${chopper}/dphas geterror
            }
	    if {[string first averl $par] >= 0} {
		set val [string range $par 5 end]
		hupdate /sics/choco/${chopper}/averl $val 
		hdelprop /sics/choco/${chopper}/averl geterror
            }
        }
    }
}
#-------------------------------------------------------------
# update those parameters which are dependent on the chopper 
# status just read. Some of them may or may not be there, this 
# is why this is protected by catch'es.
#-------------------------------------------------------------
proc astcopydependentpar {} {
    global choppers
    foreach chop $choppers {
	set val [hval /sics/choco/${chop}/aspee]
	catch {hupdate /sics/choco/${chop}/speed $val}
	set val [hval /sics/choco/${chop}/nphas]
	set dp [hval /sics/choco/${chop}/dphas]
	set val [expr $val + $dp]
	catch {hupdate /sics/choco/${chop}/phase $val}
    }
}
#--------------------------------------------------------------
proc readastriumchopperpar {} {
    global choppers 
    set reply [sct result]
    if {[string first ERR $reply] >= 0} {
	astriumchopperputerror $reply
	return idle
    }
    if {[string first "not valid" $reply] >= 0 } {
	astriumchopperputerror "ERROR: chopper responded with not valid"
	return idle
    }
    set count [sct replycount]
    if {$count == -1} {
	sct send @@NOSEND@@
	sct replycount 0
	hupdate /sics/choco/asyst ""
	hdelprop /sics/choco/asyst geterror 
	return astchoppar
    } else {
	set oldval [hval /sics/choco/asyst]
	hupdate /sics/choco/asyst "$oldval $reply"
	astriumsplitreply [lindex $choppers $count] $reply
	incr count
	sct replycount $count
	if {$count < [llength $choppers] } {
	    sct send @@NOSEND@@
	    return astchoppar
	} else {
	    astcopydependentpar
	    return idle
	}
    }
}
#--------------------------------------------------------------
proc astchopread {} {
    sct send "asyst 1"
    sct replycount -1
    return astchoppar
}
#---------------------------------------------------------------
proc astriumMakeChopperParameters {} {
    global choppers chopperparlist
    foreach chopper $choppers {
	hfactory /sics/choco/${chopper} plain spy none
	foreach par $chopperparlist {
	    set path /sics/choco/${chopper}/${par}
	    hfactory $path plain internal text
	    chocosct connect $path
        }
    }
    hfactory /sics/choco/asyst plain user text
    hsetprop /sics/choco/asyst read astchopread
    hsetprop /sics/choco/asyst astchoppar readastriumchopperpar
    hfactory /sics/choco/stop plain user int
    chocosct poll /sics/choco/asyst 60
#--------- This is for debugging
#    chocosct poll /sics/choco/asyst 10
}
#=================== write support ==============================
proc astchopwrite {prefix} {
    set val [sct target]
    sct send "$prefix $val"
    sct writestart 1
    hupdate /sics/choco/stop 0
    return astchopwritereply
}
#----------------------------------------------------------------
# Make sure to send a status request immediatly after a reply in 
# order to avoid timing problems
#----------------------------------------------------------------
proc astchopwritereply {} {
    set reply [sct result]
    if {[string first ERR $reply] >= 0} {
	sct print $reply
	hupdate /sics/choco/stop 1
	return idle
    }
    if {[string first "chopper error" $reply] >= 0} {
	sct print "ERROR: $reply"
	hupdate /sics/choco/stop 1
	return idle
    }
    if {[string first "not valid" $reply] >= 0 } {
	sct print "ERROR: chopper responded with not valid"
	hupdate /sics/choco/stop 1
	return idle
    }
    set state [sct writestart]
    if {$state  == 1} {
	sct writestart 0
	sct send "asyst 1"
	sct replycount -1
	return astchopwritereply
    } else {
	set status [readastriumchopperpar]
	if {[string first idle $status] >= 0} {
	    return idle
	} else {
	    return astchopwritereply
	}
    }
}
#--------------------------------------------------------------------
proc astchopcompare {path1 path2 delta} {
    set v1 [hval $path1]
    set v2 [hval $path2]
    if {abs($v1 - $v2) < $delta} {
	return 1
    } else {
	return 0
    }
}
#--------------------------------------------------------------------
proc astchopcheckspeed {chopper} {
    set stop [hval /sics/choco/stop]
    if {$stop == 1} {
	return fault
    }
    chocosct queue /sics/choco/asyst progress read
    set tg [sct target]
    set p1 /sics/choco/${chopper}/nspee
    set p2 /sics/choco/${chopper}/aspee
    set tst [astchopcompare $p1 $p2 50]
    if {$tst == 1 } {
	wait 1
	return idle
    } else {
	return busy
    }
}
#---------------------------------------------------------------------
proc astchopcheckphase {chopper} {
    set stop [hval /sics/choco/stop]
    if {$stop == 1} {
	return fault
    }
    chocosct queue /sics/choco/asyst progress read
    set p2 [hval /sics/choco/${chopper}/dphas]
    if {abs($p2) < .03} {
	wait 15
	return idle
    } else {
	return busy
    }
}
#---------------------------------------------------------------------
proc astchopcheckratio {} {
    global choppers
    set stop [hval /sics/choco/stop]
    if {$stop == 1} {
	return fault
    }
    set ch1 [lindex $choppers 0]
    set ch2 [lindex $choppers 1]
    chocosct queue /sics/choco/asyst progress read
    set p1 [hval /sics/choco/${ch1}/aspee]
    set p2 [hval /sics/choco/${ch2}/aspee]
    set target [sct target]
    if {$p2 < 10} {
	return busy
    }
    if {abs($p1/$p2 - $target*1.) < .3} {
	set tst 1
    } else {
	set tst 0
    }
    if {$tst == 1 } {
	wait 1
	return idle
    } else {
	return busy
    }
}
#----------------------------------------------------------------------
proc astchopstop {} {
    sct print "No real way to stop choppers but I will release"
    sct send @@NOSEND@@
    hupdate /sics/choco/stop 1
    return idle
}
#---------------------------------------------------------------------
proc astspeedread  {chopper} {
    set val [hval /sics/choco/${chopper}/aspee]
    sct update $val
    sct send @@NOSEND@@
    return idle
}
#---------------------------------------------------------------------
proc astchopspeedlimit {chidx} {
    global choppers maxspeed
    set chname [lindex $choppers $chidx]
    set val [sct target]
    if {$val < 0 || $val > $maxspeed} {
	error "Desired chopper speed out of range"
    }
    if {$chidx > 0} {
	set state [hval /sics/choco/${chname}/state]
	if {[string first async $state] < 0} {
	    error "Chopper in wrong state"
	}
    }
    return OK
}
#----------------------------------------------------------------------
proc astMakeChopperSpeed1 {var} {
    global choppers
    set ch [lindex $choppers 0]
    set path /sics/choco/${ch}/speed
    hfactory $path plain mugger float
    hsetprop $path read astspeedread $ch
    hsetprop $path write astchopwrite "nspee 1 "
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopspeedlimit 0
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckspeed $ch
    hsetprop $path priv manager
    makesctdriveobj $var $path DriveAdapter chocosct
}
#----------------------------------------------------------------------
proc astMakeChopperSpeed2 {var} {
    global choppers
    set ch [lindex $choppers 1]
    set path /sics/choco/${ch}/speed
    hfactory $path plain mugger float
    hsetprop $path read astspeedread $ch
    hsetprop $path write astchopwrite "nspee 2 "
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopspeedlimit 0
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckspeed $ch
    hsetprop $path priv manager
    makesctdriveobj $var $path DriveAdapter chocosct
}
#-----------------------------------------------------------------------
proc astchopphaselimit {} {
    set val [sct target]
    if {$val < -359.9 || $val > 359.9} {
	error "chopper phase out of range"
    }
    return OK
}
#---------------------------------------------------------------------
proc astphaseread  {chopper} {
    set val [hval /sics/choco/${chopper}/aphas]
    sct update $val
    sct send @@NOSEND@@
    return idle
}
#-----------------------------------------------------------------------
proc astMakeChopperPhase1 {var} {
    global choppers
    set ch [lindex $choppers 0]
    set path /sics/choco/${ch}/phase
    hfactory $path plain mugger float
    hsetprop $path read astphaseread $ch
    hsetprop $path write astchopwrite "nphas 1 "
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopphaselimit
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckphase $ch
    hsetprop $path priv manager
    makesctdriveobj $var $path DriveAdapter chocosct
}
#-----------------------------------------------------------------------
proc astMakeChopperPhase2 {var} {
    global choppers
    set ch [lindex $choppers 1]
    set path /sics/choco/${ch}/phase
    hfactory $path plain mugger float
    hsetprop $path read astphaseread $ch
    hsetprop $path write astchopwrite "nphas 2 "
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopphaselimit
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckphase $ch
    hsetprop $path priv manager
    makesctdriveobj $var $path DriveAdapter chocosct
}
#----------------------------------------------------------------------
proc astchopratiolimit {} {
    set val [sct target]
    if {$val < 1} {
	error "Ratio out of range"
    }
    return OK
}
#-----------------------------------------------------------------------
proc astMakeChopperRatio {var} {
    global choppers
    set ch [lindex $choppers 1]
    set path /sics/choco/${ch}/Ratio
    hdel $path
    hfactory $path plain mugger float
    chocosct connect $path
    hsetprop $path write astchopwrite "ratio 2 "
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopratiolimit
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckratio 
    makesctdriveobj $var $path DriveAdapter chocosct
}
#------------------------------------------------------------------------
proc chosta {} {
    global chopperlonglist chopperparlist choppers chopperheader
    set result "$chopperheader\n"
    append line [format "%-20s  " ""]
    set count 1
    foreach ch $choppers {
	append line [format "%-20s " $ch]
	incr count
    }
    append result $line "\n"
    set nchop [llength $choppers]
    set len [llength $chopperlonglist]
    for {set i 0} {$i < $len} {incr i} {
	set line ""
	set par [lindex $chopperlonglist $i]
	append line [format "%-20s " $par]
	for {set n 0} {$n < $nchop} {incr n} {
	    set chname [lindex $choppers $n]
	    set parname [lindex $chopperparlist $i]
	    set val [hval /sics/choco/${chname}/${parname}]
	    append line [format "%-20s " $val]
	}
	append result $line "\n"
    }
    return $result
}
#======================= Configuration Section ==========================
set amor 0
set poldi 1
set focus 0

if {$amor == 1} {
    set choppers [list chopper1 chopper2]
    set chopperparlist [list amode aspee nspee nphas dphas averl ratio vibra t_cho \
			    durch vakum valve sumsi spver state]
    set chopperlonglist [list "Chopper Mode" "Actual Speed" "Set Speed" "Phase" "Phase Error" \
			     "Loss Current" Ratio Vibration Temperature "Water Flow" Vakuum \
			     Valve Sumsi]
    set chopperheader "AMOR Chopper Status"
    makesctcontroller chocosct std psts224:3014 "\r\n" 60
#    makesctcontroller chocosct std localhost:8080 "\r\n" 60
    chocosct debug -1
    set maxspeed 5000
    set minphase 0
    astriumMakeChopperParameters
    astMakeChopperSpeed1 chopperspeed
    astMakeChopperPhase2 chopper2phase
    Publish chosta Spy
}

#----------------------------- POLDI -----------------------------------------
if {$poldi == 1} {

    proc poldiastchopphaselimit {} {
	set val [sct target]
	if {$val < 80 || $val > 100} {
	    error "chopper phase out of range"
	}
	return OK
    }
#-------
    proc poldispeedwrite {} {
	set val [sct target]
	set l [split [config myrights] =]
	set rights [string trim [lindex $l 1]]
	if {$rights == 2} {
	    if {$val < 4990 || $val > 15000} {
		clientput "ERROR: Users may only drive the chopper between 5000 - 15000 RPM"
		hupdate /sics/choco/stop 1
		return idle
            }
        }
	return [astchopwrite "nspee 1 "] 
    }
#-----------
    set choppers [list chopper]
    set chopperparlist [list amode aspee nspee nphas dphas averl ratio vibra vibax t_cho \
			    flowr vakum valve sumsi spver state]
    set chopperlonglist [list "Chopper Mode" "Actual Speed" "Set Speed" "Phase" "Phase Error" \
			     "Loss Current" Ratio Vibration "Actual Vibration" Temperature "Water Flow" Vakuum \
			     Valve Sumsi]
    set chopperheader "POLDI Chopper Status"
    makesctcontroller chocosct std psts240:3005 "\r\n" 60
#    makesctcontroller chocosct std localhost:8080 "\r\n" 60
    chocosct debug -1
    set maxspeed 15000
    set minphase 80
    astriumMakeChopperParameters
#    astMakeChopperSpeed1 chopperspeed

    set path /sics/choco/chopper/speed
    hfactory $path plain user float
    hsetprop $path read astspeedread chopper
    hsetprop $path write poldispeedwrite
    hsetprop $path astchopwritereply astchopwritereply
    chocosct write $path
    hsetprop $path checklimits astchopspeedlimit 0
    hsetprop $path halt astchopstop
    hsetprop $path checkstatus astchopcheckspeed chopper
    hsetprop $path priv user
    makesctdriveobj chopperspeed $path DriveAdapter chocosct

    astMakeChopperPhase1 chopperphase
    hsetprop /sics/choco/chopper/phase checklimit poldiastchopphaselimit
    Publish chosta Spy
}
#----------------------------- FOCUS -----------------------------------------------------
if {$focus == 1} {
    set choppers [list fermi disk]
    set chopperparlist [list state amode aspee nspee nphas dphas averl ratio vibra t_cho \
			    durch vakum valve sumsi]
    set chopperlonglist [list "Chopper State" "Chopper Mode" "Actual Speed" "Set Speed" \
			     "Phase" "Phase Error" \
			     "Loss Current" Ratio Vibration Temperature "Water Flow" \
			     Vakuum Valve Sumsi]
    set chopperheader "FOCUS Chopper Status"
    makesctcontroller chocosct std psts227:3008 "\r\n" 60
#    makesctcontroller chocosct std localhost:8080 "\r\n" 60
    chocosct debug 0
    set maxspeed 20000
    set minphase 0
    astriumMakeChopperParameters
    astMakeChopperSpeed1 fermispeed
    astMakeChopperSpeed2 diskspeed
    astMakeChopperRatio  ratio
    astMakeChopperPhase2 phase
    Publish chosta Spy 
}