#!/bin/sh  
# \
exec oagtclsh "$0" "$@"

set auto_path [linsert $auto_path 0  /usr/local/oag/apps/lib/$env(HOST_ARCH)]
set auto_path [linsert $auto_path 0 /usr/local/oag/lib_patch/$env(HOST_ARCH)]

APSStandardSetup
set args $argv
set rootname stepScan
set stepSize 0.051
set dryRun 1
set magnet ""
set timeout 5
set scanFile RG2scan.sdds
set cycles 2
APSParseArguments {rootname stepSize magnet dryRun timeout scanFile cycles}

puts "start: [exec date]"
set startTime [clock second]
package require math::statistics

if ![file exist $scanFile] {
    puts stderr "$scanFile does not exist, please make a copy from /home/helios/oagData/linac/GunOptimize/RG2/RG2scan.sdds, and remove the magnets that had already been scanned. "
    exit 1
}

proc print {text} {
    puts "[exec date] $text"
}

proc Test {args} {
    global dryRun  L1:RG2:KIK:chargeTrigC
    if $dryRun {
	#no test is needed for dryRun mode
	return
    }
    set timeout [expr [clock seconds]+30]
    while {[clock seconds]<$timeout} {
	pv getw L1:RG2:KIK:chargeTrigC
	update
	if {${L1:RG2:KIK:chargeTrigC}=="Disabled"} {
	    puts "enable RG2 trigger"
	    set L1:RG2:KIK:chargeTrigC 1
	    update
	    pv putw L1:RG2:KIK:chargeTrigC
	    after 1000
	} else {
	    break
	}
    }    
}

proc SetupFile {args} {
    set mag ""
    set root ""
    APSParseArguments {mag root}
    global pvList
    set file $root.$mag
    catch {exec rm $file}
    set fid [open $file a+]
    puts $fid "SDDS1"
    puts $fid "&column name=Step type=long &end"
    puts $fid "&column name=Time type=float &end"
    foreach pv $pvList {
	puts $fid "&column name=$pv type=float &end"
    }
    puts $fid "&data mode=ascii no_row_counts=1 &end"
    flush $fid
    return $fid
}

#set scanFile /home/helios/oagData/linac/GunOptimize/RG2/RG2scan.sdds
set magList [exec sdds2stream -col=ControlName $scanFile]
set readbackList [exec sdds2stream -col=ReadbackName $scanFile]
set startList [exec sdds2stream -col=Start $scanFile]
set stopList [exec sdds2stream -col=Stop  $scanFile]
set directionList [exec sdds2stream -col=ScanDirection  $scanFile]

set i  0
foreach mag $magList {
    set scan(readback.$mag) [lindex $readbackList $i]
    set scan(start.$mag) [lindex $startList $i]
    set scan(stop.$mag) [lindex $stopList $i]
    set scan(dir.$mag) [lindex $directionList $i]
    incr i
}

if ![string length $magnet] {
       # set magList1 [exec sddsprocess $scanFile -pipe=out \
       #     "-match=col,ControlName=L1:SC3*,!,ControlName=L1:SC4*,!,&" | sdds2stream -pipe -col=ControlName]
       # set readbackList1 [regsub -all "CurrentAO" $magList "CurrentAI"]
    set magList1 $magList
} else {
    set magList1 $magnet
}

set pvList {L1:CM2:measCurrentCM L3:CM1:measCurrentCM}
set pvList [concat $pvList $magList $readbackList]

pv linkw $pvList $pvList
pv linkw L1:RG2:KIK:chargeTrigC  L1:RG2:KIK:chargeTrigC

set timestr [clock format [clock seconds] -format %y-%m%d:%H%M%S]
set root ${rootname}-$timestr

#read current value
if [catch {exec cavget -list=[join $magList1 ,] -pend=30} valList] {
    puts stderr "Error reading $magList: $valList"
    exit 1
}

foreach mag $magList1 val $valList {
    set init($mag) [format %.3f $val]
}

set steps [expr int(1.0/$stepSize)]

set index [lsearch -exact $magList "L1:SC3:VL:CurrentAO"]
set i -1
foreach mag $magList1 {
    puts "ready to scan $mag? press enter to continue."
    gets stdin userInput
    set readback $scan(readback.$mag)
    puts "scan $mag, readback: $readback"
    
    set magValList ""
    #first go to -1A delta
    lappend magValList $init($mag)
    set start $scan(start.$mag)
    set stop $scan(stop.$mag)
    set direction $scan(dir.$mag)
    set val $init($mag)
    #need do 2 cycles
    for {set cycle 0} {$cycle<$cycles} {incr cycle} {
	if $direction==-1 {
	    #go to -2A
	    while {1} {
		set val [expr $val - $stepSize]
		if {$val>$start} {
		    lappend magValList [format %0.3f $val]
		} else {
		    set val [format %.3f [expr $val + $stepSize]]
		    break
		}
	    }
	    #go to 2A
	    while {1} {
		set val [format %0.3f [expr $val+$stepSize]]
		if {$val<$stop} {
		    lappend magValList $val
		} else {
		    set val [format %.3f [expr $val - $stepSize]]
		    break
		}
	    }
	    #go back it orignal value
	    while {1} {
		set val [format %.3f [expr $val-$stepSize]]
		if {$val>=$init($mag)} {
		    lappend magValList $val
		} else {
		    break
		}
	    }
	    set last [lindex $magValList end]
	    if {$last>$init($mag)} {
		lappend magValList $init($mag)
	    }
	} else {
	    #go to stop
	    while {1} {
		set val [expr $val + $stepSize]
		if {$val<$stop} {
		    lappend magValList $val
		} else {
		    set val [expr $val - $stepSize]
		    break
		}
	    }
	    
	    #go to start
	    while {1} {
		set val [expr $val - $stepSize]
		if {$val>$start} {
		    lappend magValList $val
		} else {
		    set val [expr $val + $stepSize]
		    break
		}
	    }
	    #go to initial 
	    while {1} {
		set val [expr $val + $stepSize]
		if {$val<$init($mag)} {
		    lappend magValList $val
		} else {
		    set val [expr $val - $stepSize]
		    break
		}
	    }
	    set last [lindex $magValList end]
	    if {$last<$init($mag)} {
		lappend magValList $init($mag)
	    }
	}
    }
    puts $magValList
    #before scan, disable and enable kicker to avoid kicker timout
    if !$dryRun {
	Test
    }
    set fid [SetupFile -mag $mag -root $root]
    set L3chargeMean ""
    set L3chargeStd ""
    set L3chargeSpread ""
    set timeList ""
    set step 1
    foreach val $magValList {
        puts "step $step : set $mag to $val..."
        set ${mag} $val
	if !$dryRun {
	   pv putw ${mag}
	}
	set timeout1 [expr $timeout + [clock seconds]]
        while {[clock seconds]<$timeout1} {
            pv get ${readback}
            update
            set tol [expr abs([set ${readback}] - $val)]
            puts "readback-setpoint now: $tol"
            if {$tol<0.01} {
                break
            }
            after 500
        }
	pv get ${readback}
	update
	set tol [expr abs([set ${readback}] - $val)]
	if {$tol>0.01} {
	    puts "Error: $mag and $readback do not agree, the data collected for $mag might be corrupted."
	}
        #wait for steering controllaw
	after 1000
	#wait for L3 chargto stablize -- waiting for steering controllaw to converge
        while {1} {
	    Test
            if [catch {exec cavget -list=L3:CM1:measCurrentCM -stat=number=5,pause=0.4,format=tag} L3CM1] {
		return -code "Error reading L3CM1 current: $L3CM1"
	    }
            array set current $L3CM1
	    set L3:CM1:measCurrentCM $current(mean)
            puts "L3CM1 current spread now $current(spread)"
            if {$dryRun || $current(spread)<0.05} {
		#dryRun do not check L3CM1
                break
            }
            after 500
        }
        pv getw $pvList
	set L3:CM1:measCurrentCM $current(mean)
        update
	set valList $step
	lappend valList [clock seconds]
        foreach pv $pvList {
	    lappend valList [set $pv]
        }
	puts $fid $valList
	flush $fid
	incr step
    }
    close $fid
    puts "$mag scan done, please re-conditioning RG2 magnet before doing scan for next magnet..."
    puts "After conditioning is done, press enter to continue"
    gets stdin userInput
}

set endTime [clock seconds]
puts "took [expr ($endTime-$startTime)/60.] minutes."

exit 0



