#---------------------------------------------------------------------------
# peaksearch a peak search utility for TRICS using the PSD detectors.
#
# Mark Koennecke, November 2001
#---------------------------------------------------------------------------

proc initPeakSearch {} {

    #------- phi Range
    VarMake ps.phiStart Float User
    ps.phiStart 0
    VarMake ps.phiEnd Float User
    ps.phiEnd 180
    VarMake ps.phiStep Float User
    ps.phiStep 3.

    #-------- chi range
    VarMake ps.chiStart Float User
    ps.chiStart 0
    VarMake ps.chiEnd Float User
    ps.chiEnd 180
    VarMake ps.chiStep Float User
    ps.chiStep 12.

    #-------- omega range
    VarMake ps.omStart Float User
    ps.omStart 0
    VarMake ps.omEnd Float User
    ps.omEnd 30
    VarMake ps.omStep Float User
    ps.omStep 3.

    #------- two theta range
    VarMake ps.sttStart Float User
    ps.sttStart 5
    VarMake ps.sttEnd Float User
    ps.sttEnd 70
    VarMake ps.sttStep Float User
    ps.sttStep 3.

    #------- maximum finding parameters
    VarMake ps.threshold Int User
    ps.threshold 30
    VarMake ps.steepness Int User
    ps.steepness 3
    VarMake ps.window Int User
    ps.window 7
    VarMake ps.cogwindow Int User 
    ps.cogwindow 60
    VarMake ps.cogcontour Float User
    ps.cogcontour .2

    #-------- counting parameters
    VarMake ps.countmode Text User
    ps.countmode monitor
    VarMake ps.preset Float User
    ps.preset 1000

    #-------- final scan counting parameters
    VarMake ps.scanpreset Float User
    ps.scanpreset 1000000
    VarMake ps.scansteps Int User
    ps.scansteps 24

    #--------- file to which to write the results
    VarMake ps.listfile Text User
    ps.listfile peaksearch.dat 

    #--------- conversion factors from Pixel to mm
    VarMake xfactor Float Mugger
    xfactor 0.715
    VarMake yfactor Float Mugger 
    yfactor 1.42

    #--------- published functions
    Publish ps.phirange User
    Publish ps.chirange User
    Publish ps.omrange User
    Publish ps.sttrange User
    Publish ps.countpar User
    Publish ps.scanpar  User
    Publish ps.maxpar   User
    Publish ps.list     User
    Publish ps.listpeaks User
    Publish ps.run       User
    Publish ps.continue  User
    Publish ps.scanlist  User
#------- these are for debugging only!
    Publish checkomega User
    Publish optimizedetector User
    Publish optimizepeak User
    Publish printpeak User
    Publish initsearch User
    Publish catchdrive User
    Publish catchdriveval User
    Publish scandetectorsD User
    Publish scandetectors User
}
#--------------------------------------------------------------------------
proc ps.phirange args {
    if { [llength $args] >= 3 } {
	ps.phiStart [lindex $args 0]
	ps.phiEnd  [lindex $args 1]
        ps.phiStep [lindex $args 2]
    }
    clientput "Peak Search Phi Range:"
    return [format "   Start = %6.2f, End = %6.2f, Step = %6.2f" \
           [SplitReply [ps.phiStart]] [SplitReply [ps.phiEnd]] \
           [SplitReply [ps.phiStep]]] 	
}
#--------------------------------------------------------------------------
proc ps.chirange args {
    if { [llength $args] >= 3 } {
	ps.chiStart [lindex $args 0]
	ps.chiEnd  [lindex $args 1]
        ps.chiStep [lindex $args 2]
    }
    clientput "Peak Search Chi Range:"
    return [format "   Start = %6.2f, End = %6.2f, Step = %6.2f" \
           [SplitReply [ps.chiStart]] [SplitReply [ps.chiEnd]] \
           [SplitReply [ps.chiStep]]] 	
}
#--------------------------------------------------------------------------
proc ps.omrange args {
    if { [llength $args] >= 3 } {
	ps.omStart [lindex $args 0]
	ps.omEnd  [lindex $args 1]
        ps.omStep [lindex $args 2]
    }
    clientput "Peak Search Omega Range:"
    return [format "   Start = %6.2f, End = %6.2f, Step = %6.2f" \
           [SplitReply [ps.omStart]] [SplitReply [ps.omEnd]] \
           [SplitReply [ps.omStep]]] 	
}
#--------------------------------------------------------------------------
proc ps.sttrange args {
    if { [llength $args] >= 3 } {
	ps.sttStart [lindex $args 0]
	ps.sttEnd  [lindex $args 1]
        ps.sttStep [lindex $args 2]
    }
    clientput "Peak Search Two Theta Range:"
    return [format "   Start = %6.2f, End = %6.2f, Step = %6.2f" \
           [SplitReply [ps.sttStart]] [SplitReply [ps.sttEnd]] \
           [SplitReply [ps.sttStep]]] 	
}
#-------------------------------------------------------------------------
proc ps.countpar args {
    if { [llength $args] >= 2 } {
        if { [catch {counter setmode [lindex $args 0]} msg] != 0} {
	    error "ERROR: Invalid countmode specified"
        } 
	ps.countmode [lindex $args 0]
	ps.preset  [lindex $args 1]
    }
    clientput "Peak Search Count Parameters:"
    return [format "   Mode = %s, Preset = %12.2f" \
           [SplitReply [ps.countmode]] [SplitReply [ps.preset]]] 
}
#-------------------------------------------------------------------------
proc ps.scanpar args {
    if { [llength $args] >= 2 } {
	ps.scanpreset [lindex $args 0]
	ps.scansteps  [lindex $args 1]
    }
    clientput "Peak Search Scan Parameters:"
    return [format "   Count Preset = %12.2f, No. Steps %4d" \
           [SplitReply [ps.scanpreset]] [SplitReply [ps.scansteps]]] 
}
#-------------------------------------------------------------------------
proc ps.maxpar args {
    if { [llength $args] >= 5 } {
	ps.window [lindex $args 0]
	ps.threshold  [lindex $args 1]
	ps.steepness [lindex $args 2]
	ps.cogwindow  [lindex $args 3]
	ps.cogcontour  [lindex $args 4]
    }
    clientput "Peak Search Maximum Detection Parameters:"
    set t1 [format "   Window = %d, Threshold = %d * average, Steepness = %d" \
           [SplitReply [ps.window]] [SplitReply [ps.threshold]] \
           [SplitReply [ps.steepness] ]]
    set t2 [format "   COGWindow = %d, COGcontour = %f " \
           [SplitReply [ps.cogwindow]] [SplitReply [ps.cogcontour]]] 
    return [format "%s\n%s" $t1 $t2]
}
#-----------------------------------------------------------------------
proc ps.list {} {
    clientput [ps.sttrange]
    clientput [ps.omrange]
    clientput [ps.chirange]
    clientput [ps.phirange]
    clientput [ps.countpar]
    clientput [ps.scanpar]
    clientput [ps.maxpar]
}
#------------------------------------------------------------------------
proc string2list {txt} {
    return [split [string trim $txt \{\}]]
}
#------------------------------------------------------------------------
proc checknewomega {hm x y omega maxIntensity} {
  if {[catch {drive om $omega} msg] != 0} {
     error $msg
  }
  if {[catch {hmc start [SplitReply [ps.preset]] [string trim [SplitReply \
            [ps.countmode]]]} msg] != 0} {
     error $msg
  }
  if {[catch {Success}  msg] != 0} {
     error $msg
  }
  if { [catch {lomax cog $hm $x $y} result] != 0} {
	     error "Failed to calculate COG: $result"
  }
  set result [split $result " "]
  if {[lindex $result 2] > $maxIntensity } {
      return $result
  } else {
      return 0
  }
}
#------------------------------------------------------------------------
proc optimizedetector {hm} {
    if { [catch {lomax stat $hm} result] != 0} {
#--- This can be due to the fact that the detector is missing. Sigh ....
	return
    }  
    set l2 [split [string trim $result]]
    lomax threshold [expr [lindex $l2 0] * [SplitReply [ps.threshold]]]
    set result [lomax search $hm] 
    set oldom [SplitReply [om]]
    set result [split $result @]
    for {set i 0} { $i < [llength $result]} {incr i} {
	if { [catch {drive om $oldom} msg] != 0} {
	    error $msg
        }
        set piecks [split [lindex $result $i] " "]
	set x [lindex $piecks 0]
	set y [lindex $piecks 1]
	if { [catch {optimizepeak $hm $x $y} msg] != 0} {
	    clientput [format "Aborted peak at %d %d with %s" $x $y $msg]
        }
    } 
}
#------------------------------------------------------------------------
proc optimizepeak {hm x y} {
    if { [catch {lomax cog $hm $x $y} result] != 0} {
	error "Failed to calculate COG: $result"
    }
    set result [split $result " "]
    set xMax [lindex $result 0]
    set yMax [lindex $result 1]
    set maxIntensity [lindex $result 2]
    set maxOmega [SplitReply [om]]
    set startOmega $maxOmega
    set omSearchStep .1
#--------- move to positive omega until maximum found
    while {1} {
	set newOm [expr [SplitReply [om]] + $omSearchStep]
        if {[catch {checknewomega $hm $xMax $yMax $newOm $maxIntensity} \
                       result] != 0} {
	    error $result
        }
        if {$result != 0} {
	    set xMax [lindex $result 0]
            set yMax [lindex $result 1]
            set maxIntensity [lindex $result 2]
            set maxOmega [SplitReply [om]]
        } else {
	    break
        }
    }
#--------- if maxOmega is still startOmega then we were on the right
#          side of the peak. In this case try to find the maximum in
#          negative direction
    if {$maxOmega == $startOmega} {
       while {1} {
	   set newOm [expr [SplitReply [om]] - $omSearchStep]
           if {[catch {checknewomega $hm $xMax $yMax $newOm $maxIntensity} \
                        result] != 0} {
	      error $result
           }
           if {$result != 0} {
	       set xMax [lindex $result 0]
	       set yMax [lindex $result 1]
	       set maxIntensity [lindex $result 2]
	       set maxOmega [SplitReply [om]]
	   } else {
	       break
	   }
       }
    } 
#----------- print the results we have found
   printpeak $hm $xMax $yMax $maxOmega $maxIntensity
#------------ scan the peak for Oksana
#     set scanStart [expr $maxOmega - 0.1*([SplitReply [ps.scansteps]]/2)]
#     if { [catch {tricsscan $scanStart .1 [SplitReply [ps.scansteps]] \
#      [SplitReply [ps.countmode]] [SplitReply [ps.scanpreset]]} msg] } {
# 	error $msg
#     }
}
#---------------------------------------------------------------------------
proc printpeak {hm x y om intensity} {
    set phval [SplitReply [phi]]
    set chval [SplitReply [chi]]
    set gamOffset .0
    switch $hm {
	hm1 {
               set tilt [SplitReply [dg1]]
               set gamOffset .0
            }
        hm2 {
               set tilt [SplitReply [dg2]]
	       set gamOffset 0.
            }
	hm3 {
               set tilt [SplitReply [dg3]]
	       set gamOffset 45.
            }
	default {error "Invalid hm requested in printpeak"}
    }
    set sttval [expr [SplitReply [stt]] + $gamOffset]
    set zero [SplitReply [$hm configure dim0]]
    set xval [expr $x * [SplitReply [xfactor]]]
    set zero [SplitReply [$hm configure dim1]]
    set yval [expr $y * [SplitReply [yfactor]]]
    set line [format "%7.2f%7.2f%7.2f%7.2f%7.2f%7.2f%7.2f%10d" \
      $xval $yval $sttval $om $chval $phval $tilt $intensity]
    clientput "Found Peak at:"
    clientput $line
    set f [open [string trim [SplitReply [ps.listfile]]] a+]
    puts $f $line
    close $f
}
#--------------------------------------------------------------------------
proc ps.listpeaks {} {
    clientput "Peakse found so far: "
    clientput "      X      Y    STT     OM    CHI    PHI   TILT INTENSITY" 
    set f [open [string trim [SplitReply [ps.listfile]]] r]
    while {[gets $f line] > 0} {
	clientput [format "%s" $line]
    }
    close $f
}
#-------------------------------------------------------------------------
proc initsearch {filename} {
#----- stow away filename and empty it
    ps.listfile $filename
    set f [open $filename w]
    close $f
#----- tell lomax its parameters
    lomax threshold  [SplitReply [ps.threshold]]
    lomax steepness  [SplitReply [ps.steepness]]
    lomax window     [SplitReply [ps.window]]
    lomax cogwindow  [SplitReply [ps.cogwindow]]
    lomax cogcontour [SplitReply [ps.cogcontour]]
#----- drive to start
    if { [catch {drive stt [SplitReply [ps.sttStart]] \
                 om [SplitReply  [ps.omStart]] \
                 chi [SplitReply [ps.chiStart]] \
                 phi [SplitReply [ps.phiStart]] } msg] != 0} {
	 error $msg
     }  
}
#------------------------------------------------------------------------
# This code means: ignore any errors except interrupts when searching

proc scandetectors {} {
#    set names [list hm1 hm2 hm3]
    set names [list hm2 hm3]
    if {[catch {hmc start [SplitReply [ps.preset]] [string trim [SplitReply \
                    [ps.countmode]]]} msg] != 0} {
	if{[string compare [getint] continue] != 0} {
          error $msg
        } else {
	  clientput [format "Ignoring: %s" $msg]
	}
    }
    if {[catch {Success}  msg] != 0} {
	if{[string compare [getint] continue] != 0} {
          error $msg
        } else {
	  clientput [format "Ignoring: %s" $msg]
	}
    }
    for {set i 0} { $i < [llength $names]} {incr i} {
	set ret [catch {optimizedetector [lindex $names $i]} msg]
	if { $ret != 0} {
	    if {[string compare [getint] continue] != 0} {
		error $msg
            } else {
		clientput [format "Ignoring problem: %s" $msg]
            }
        }
    }   
}
#------------------------------------------------------------------------
# loop debugging

proc scandetectorsD {} {
    clientput [format "stt = %6.2f, om = %6.2f, chi = %6.2f, phi = %6.2f" \
	    [SplitReply [stt]] [SplitReply [om]]  \
            [SplitReply [chi]] [SplitReply [phi]]]
    wait 1
} 
#-----------------------------------------------------------------------
proc catchdrive { mot step} {
    set ret [catch {drive $mot [expr [SplitReply [$mot]] + $step]} msg]
    if {$ret != 0} {
	if {[string compare [getint] continue] != 0} {
          error $msg
        } else {
	  clientput [format "Ignoring: %s" $msg]
	}
    }
}
#-----------------------------------------------------------------------
proc catchdriveval { mot val} {
    set ret [catch {drive $mot $val} msg]
    if {$ret != 0} {
	if {[string compare [getint] continue] != 0} {
          error $msg
        } else {
	  clientput [format "Ignoring: %s" $msg]
	}
    }
}
#------------------------------------------------------------------------
# The actual loop. It is written in a way which allows for the continuation
# of a search

proc searchloop { } {
    set sttStep [SplitReply [ps.sttStep]]
    set sttEnd  [SplitReply [ps.sttEnd]]
    set chiStep [SplitReply [ps.chiStep]]
    set chiEnd  [SplitReply [ps.chiEnd]]
    set phiStep [SplitReply [ps.phiStep]]
    set phiEnd  [SplitReply [ps.phiEnd]]
    set omStep  [SplitReply [ps.omStep]]
    set omEnd   [SplitReply [ps.omEnd]]
    while {[SplitReply [stt]] + $sttStep <= $sttEnd}  {
	while {[SplitReply [chi]] + $chiStep <= $chiEnd} {
	    while {[SplitReply [om]] + $omStep <= $omEnd} {
		while {[SplitReply [phi]] + $phiStep <= $phiEnd} {
		    scandetectors
		    catchdrive phi $phiStep
		}
		catchdrive om $omStep
		catchdriveval phi [SplitReply [ps.phiStart]]
            }
	    catchdrive chi $chiStep
	    catchdriveval om [SplitReply [ps.omStart]]
        }
        catchdrive stt $sttStep
	catchdriveval chi [SplitReply [ps.chiStart]]
    }
    return "Peak Search finished normally" 
}
#---------------------------------------------------------------------------
proc ps.run {filename} {
    initsearch $filename
    searchloop
}
#-------------------------------------------------------------------------
proc ps.continue {} {
    searchloop
}
#------------------------------------------------------------------------
proc ps.scanlist {} {
    if { [catch {set f [open [string trim [SplitReply [ps.listfile]]] "r"]} \
	    msg ] != 0} {
	error $msg
    }
    while { [gets $f line] > 0} {
	set n [stscan $line "%f %f %f %f %f %f" x y stt om chi phi]
	if {$n < 6} {
	    clientput [format "Skipping invalid line: %s" line]
	    continue
        }
	if { [catch {drive stt $stt om $om chi $chi phi $phi} msg] != 0 } {
	    clientput $msg
	    if {[string compare [getint] continue] != 0} {
		error "ps.scanlist interupted"
            }
        }
        set scanStart [expr $om - 0.1*([SplitReply [ps.scansteps]]/2)]
        if { [catch {tricsscan $scanStart .1 [SplitReply [ps.scansteps]] \
             [SplitReply [ps.countmode]] [SplitReply [ps.scanpreset]]} msg] \
             != 0 } {
 	     clientput $msg
 	     if {[string compare [getint] continue] != 0} {
		error "ps.scanlist interupted"
             }
        }
    }     
    close $f
    return "Scanning list finished"
}