#!/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 sector 1
set ID ID1
set plane x
set window Both
set logFile ""
set P0L 2.756
set P1L 3.213
set testType offset
set fastAbort 0
set BPMconfig default
set weed 1
set direction 1
set method 2
set ID1exist 1
set ID2exist 1
set tolerance 200
set reset 1
set fastAbortPoint 1
#fastabort only test one point
set outDir [APSGoToDailyDirectory -subdirectory IDValidation]
APSParseArguments {sector ID L plane window logFile outDir testType fastAbort BPMconfig weed direction method ID1exist ID2exist tolerance reset fastAbortPoint}

set BPLD(fastAbortPoint) $fastAbortPoint
set BPLD(reset) $reset
set BPLD(method) $method
set BPLD(tolerance) $tolerance
#on helios, orbit check does not work now
puts "S$sector direction=$direction"

if [regexp {head} $env(HOSTNAME)] {
    #weed
    set BPLD(noOrbitCheck) 0
    set BPLD(validate) 0
    set BPLD(weed) 1
} else {
       #helios orbit not available yet
    if {$testType=="offset"} {
	set BPLD(noOrbitCheck) 1
    } else {
	set BPLD(noOrbitCheck) 0
    }
    set BPLD(validate) 1
    set BPLD(weed) 0
} 

#only x or y plane is allowed
switch $plane {
    x -
    h {
	set BPLD(coord) x
	set BPLD(plane) x
    } 
    v -
    y {
	set BPLD(coord) y
	set BPLD(plane) y
    }
    default {
	return -code error "Invalid plane ($plane) provided"
    }
}

#direction=1, scan from point 1 to point 12
#direction=-1, scan from point 7,8,9,10,11,12,1,2,3,4,5,6
set BPLD(direction) $direction
set BPLD(P0L) $P0L
set BPLD(P1L) $P1L
set BPLD(BPMconfig) $BPMconfig
if {$ID=="Both"} {
    if {$ID1exist && $ID2exist} {
	set IDList {ID1 ID2}
    } elseif $ID1exist {
	set IDList ID1
    } elseif $ID2exist {
	set IDList ID2
    } else  {
	return -code error "No ID exists for sector $sector!"
    }
} else {
    if ![set ${ID}exist] {
	return -code error "secto $sector $ID does not exist"
    }
    set IDList $ID
}

if {$sector==1} {
    #for S01, ID1 (which is ID40) does not exist, only ID2 is available for validating
    set IDList ID2
    set ID ID2
}

if {$plane=="Both"} {
    set planeList {x y}
} else {
    set planeList $BPLD(plane)
}
set BPLD(testType) $testType
set BPLD(planeList) $planeList
set BPLD(IDList) $IDList
set BPLD(fastAbort) $fastAbort
if ![string length $logFile] {
    set BPLD(logFile) /tmp/[APSTmpString]
    APSAddToTmpFileList -ID IDbump -fileList $BPLD(logFile)
} else {
    set BPLD(logFile) $logFile
}

set BPLD(configDir) /home/helios/oagData/sr/BPLD/BPLD_Config

#set BPLD(plane) $plane
proc ReadBPLDConfig {args} {
    global BPLD lab
    if {$BPLD(BPMconfig)=="default"} {
	set configFile $BPLD(configDir)/IDconfig.sdds
    } else {
	set configFile  $BPLD(configDir)/BPLDConfig-ID$BPLD(BPMconfig).sdds
    }
    puts $configFile
    set tmpRoot /tmp/[APSTmpString]
    APSAddToTmpFileList -ID id_bpld -fileList $tmpRoot.1
    foreach ID $BPLD(IDList) {
	exec  $BPLD(configDir)/scripts/generateIDBPLDConfigPointBySector -weed $BPLD(weed) -sector $BPLD(sector) \
	    -id $ID -configDir $BPLD(configDir)
	set IDconfig  $BPLD(configDir)/ID_BPLD_Config.S$BPLD(sectorf)$ID.sdds
	APSAddToTmpFileList -ID idbpld -fileList "$tmpRoot.$ID.x $tmpRoot.$ID.y"
	foreach plane {x y} {
	    if [catch {exec sddsprocess $IDconfig -match=par,plane=$plane $tmpRoot.$ID.$plane } result] {
		return -code error "Error process config file: $result"
	    }
	    set BPLD($ID.${plane}OffsetLimit) [expr 1000.0 * [exec sdds2stream -par=OffsetLimit $tmpRoot.$ID.$plane]]
	    set BPLD($ID.${plane}AngleLimit) [expr 1000.0 * [exec sdds2stream -par=AngleLimit $tmpRoot.$ID.$plane]]
	}
#	APSAddToTmpFileList -ID bpld -fileList "$tmpRoot.$ID.x $tmpRoot.$ID.y"
    }
    if [catch {exec sddsprocess $configFile -filter=col,BPLDSector,$BPLD(sector),$BPLD(sector) -filter=col,IDBPM,1,1  $tmpRoot.1} result] {
	return -code error "Error1: $result"
    }
    set bpmList [exec sdds2stream -col=DeviceName $tmpRoot.1]
    set indexList [exec sdds2stream -col=BPLDIndex $tmpRoot.1]
    #read BPM index, and ID distance value from firmware
    if !$BPLD(weed) {
	#read BPM index, and ID distance value from firmware
	foreach ID $BPLD(IDList) {
	    if [catch {exec cavget -list=S$BPLD(sectorf)-LMPS:BPLD: -list=$ID \
			   -list=:BPM- -list=XY1,XY2 -list=_idx-SP -pend=20} valList] {
		return -code error "Error reading $BPLD(sector) $BPLD(IDchoice) bpm index: $valList"
	    }
	    
	    set i1 [lsearch -exact $indexList [lindex $valList 0]]
	    set i2 [lsearch -exact $indexList [lindex $valList 1]]
	    puts "$i1 $i2 $valList"
	    if {$i1<0 || $i2<0} {
		return -code error "Error: bpm index not found in BPLD config"
	    }
	    set bpm1 [lindex $bpmList $i1]
	    set bpm2 [lindex $bpmList $i2]
	    set bpm1 [lindex $bpmList $i1]
	    set bpm2 [lindex $bpmList $i2]
	    set BPLD($ID.bpm1) $bpm1
	    set BPLD($ID.bpm2) $bpm2
	    set BPLD($ID.useBPMs) [list $bpm1 $bpm2]
	}
    } else {
	set BPLD(ID1.bpm1) [lindex $bpmList 0]
	set BPLD(ID1.bpm2) [lindex $bpmList 1]
	set BPLD(ID2.bpm1) [lindex $bpmList 2]
	set BPLD(ID2.bpm2) [lindex $bpmList 3]
	set BPLD(ID1.useBPMs) [lrange $bpmList 0 1]
	set BPLD(ID2.useBPMs) [lrange $bpmList 2 3]
    }

    foreach ID $BPLD(IDList) {
	set bpm1 $BPLD($ID.bpm1)
	set bpm2 $BPLD($ID.bpm2)
	set type ""
	if [regexp {P0} $bpm1] {
	    set type P0
	    set BPLD($ID.L1) $BPLD(P0L)
	} else {
	    set type P1
	    set BPLD($ID.L1) $BPLD(P1L)
	}
	if [regexp {P0} $bpm2] {
	    append type P0
	    set BPLD($ID.L2) $BPLD(P0L)
	} else {
	    append type P1
	    set BPLD($ID.L2) $BPLD(P1L)
	}
	set BPLD($ID.L1) [expr -1.0*$BPLD($ID.L1)]
	set BPLD($ID.IDL) [expr $BPLD($ID.L2) - $BPLD($ID.L1)]
	if !$BPLD(weed) {
	    #read L from firmware
	    if [catch {exec cavget -list=S$BPLD(sectorf)-LMPS:BPLD:$ID -list=:S1,:S2 -list=-SP -pend=10 } valList] {
		return -code error "Error reading ID L: $valList"
	    }
	    set BPLD($ID.L1) [lindex $valList 0]
	    set BPLD($ID.L2) [lindex $valList 1]
	    set BPLD($ID.IDL) [expr $BPLD($ID.L2) - $BPLD($ID.L1)]
	}
	set pointList [join [exec sdds2stream -par=${type}DiamondPoint $tmpRoot.$ID.$BPLD(plane)]]
	puts "$ID points: $pointList"
	set offsetLimit [expr $BPLD($ID.$BPLD(plane)OffsetLimit)/1000.0]
	set angleLimit [expr $BPLD($ID.$BPLD(plane)AngleLimit)/1000.0]
	
	set BPLD($ID.IDDiamond.S0.1x) [expr -1.0*$offsetLimit]
	set BPLD($ID.IDDiamond.S0.1x') 0
	set BPLD($ID.IDDiamond.S0.2x) [lindex $pointList 0]
	set BPLD($ID.IDDiamond.S0.2x') [lindex $pointList 1]
	set BPLD($ID.IDDiamond.S0.3x) $offsetLimit
	set BPLD($ID.IDDiamond.S0.3x') 0
	set BPLD($ID.IDDiamond.S0.4x) [expr -1.0 * [lindex $pointList 0]]
	set BPLD($ID.IDDiamond.S0.4x') [expr -1.0 * [lindex $pointList 1]]

	set xList [exec sdds2stream -col=${type}_x $tmpRoot.$ID.$BPLD(plane)]
	set xpList [exec sdds2stream -col=${type}_xp $tmpRoot.$ID.$BPLD(plane)]
	set i 1
	foreach x $xList xp $xpList {
	    set BPLD($ID.IDBoth.S0.${i}x) $x
	    set BPLD($ID.IDBoth.S0.${i}x') $xp 
	    set BPLD($ID.IDBoth.S0.${i}y) $x
	    set BPLD($ID.IDBoth.S0.${i}y') $xp 
	    incr i 
	}
    
	foreach coord {x y} {
	    set BPLD($ID.IDRect.S0.1$coord) [expr -1.0*$offsetLimit]
	    set BPLD($ID.IDRect.S0.1${coord}') $angleLimit
	    set BPLD($ID.IDRect.S0.2${coord}) $offsetLimit
	    set BPLD($ID.IDRect.S0.2${coord}') $angleLimit
	    set BPLD($ID.IDRect.S0.3${coord}) $offsetLimit
	    set BPLD($ID.IDRect.S0.3${coord}') [expr -1.0*$angleLimit]
	    set BPLD($ID.IDRect.S0.4${coord}) [expr -1.0*$offsetLimit]
	    set BPLD($ID.IDRect.S0.4${coord}') [expr -1.0*$angleLimit]
	}
	foreach i {1 2 3 4} {
	    set BPLD($ID.IDDiamond.S0.${i}y) $BPLD($ID.IDDiamond.S0.${i}x)
	    set BPLD($ID.IDDiamond.S0.${i}y') $BPLD($ID.IDDiamond.S0.${i}x')
	}
    }
}


set BPLD(testType) $testType
set BPLD(window) $window
set BPLD(IDchoice) $ID
set BPLD(sectorf) [format %02d $sector]
set BPLD(sector) $sector
set BPLD(sector1) $sector
set BPLD(sector2) [expr $sector+1]
set BPLD(fsector1) [format %02d $sector]
set BPLD(fsector2) [format %02d $BPLD(sector2)]
#set BPLD(plane) $plane
set BPLD(BPLDNumber) [format %02d $sector]
#window choice: Diamond, Rect, Both
set BPLD(window) $window

#read BPLDconfig and limit
ReadBPLDConfig
set bpmList ""
set waitFor ""
foreach ID $BPLD(IDList) {
    lappend bpmList $BPLD($ID.bpm1)
    lappend bpmList $BPLD($ID.bpm2)
    if [string length $waitFor] {
	append waitFor " -and"
    }
    append waitFor " -waitFor=$BPLD($ID.bpm1):$BPLD(coord):LowPass1sErrorM,lowerLimit=-50,upperLimit=50"
    
    append waitFor " -waitFor=$BPLD($ID.bpm2):$BPLD(coord):LowPass1sErrorM,lowerLimit=-50,upperLimit=50 -and"
}
set restoreList ""
if [catch {exec cavget -list=[join $bpmList ,] -list=:$BPLD(coord):SetpointC -pend=10} origList] {
    puts stderr "Error reading $bpmList setpoint: $origList"
    exit 1
}
puts "$bpmList, original setpoints $origList"
foreach bpm $bpmList val $origList {
    lappend restoreList ${bpm}:$BPLD(coord):SetpointC=$val
}
set BPLD(bpmList) $bpmList
set BPLD(waitFor) $waitFor
#puts $BPLD(waitFor)

proc SetDPBPLDStatus {text} {
    puts $text
    flush stdout
}

proc SetupValidation {args} {
    global BPLD
    set BPLD(userResetPV) S$BPLD(BPLDNumber)-LMPS:UsrReset-SP
    set BPLD(IDglitchTurnsPV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID-GlitchTurns-SP
    set BPLD(ID1.enablePV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID1_Enable-SP
    set BPLD(ID2.enablePV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID2_Enable-SP
   
    foreach ID {ID1 ID2} {
	foreach nm {H1 H2 V1 V2} bpm {X1 X2 Y1 Y2} {
	    set BPLD($ID.diamond${nm}LimitPV) S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOL-${nm}-SP
	    set BPLD($ID.offset${nm}PV) S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${nm}-SP
	    set BPLD($ID.raw${nm}PV) S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}_${bpm}-I
	}
	foreach plane {H V} {
	    set BPLD($ID.rect${plane}OffsetLimitPV) S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOL-${plane}3-SP
	    set BPLD($ID.rect${plane}AgnleLimitPV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID1:AIAL_${plane}-SP
	}
	set BPLD($ID.faultPV) S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}_IL_RegOut-I
    }

    set sector $BPLD(sector)
    set fsector [expr ($sector+1)/2-1] 
    set Bit [format %X $fsector]
    set BPLD(main2fastPV) APSU:MMPS2:SMA:Data10-I.B$Bit
    set BPLD(main1fastPV) APSU:MMPS1:LMPS:FastAbort-Sts.B$Bit

    set BPLD(linkFaultPV) APSU:MMPS1:LMPS[expr ($sector+1)/2]:Fault-I
    set BPLD(localMPSPV) S$BPLD(BPLDNumber)-LMPS:MLinkTXpkt_FS-I
    set BPLD(IDfaultPV) S$BPLD(BPLDNumber)-LMPS:MLinkTXpkt_FS-I.B1
    set BPLD(ID1faultPV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID1_IL_RegOut-I
    set BPLD(ID2faultPV) S$BPLD(BPLDNumber)-LMPS:BPLD:ID2_IL_RegOut-I

    set nmList [array names BPLD]
    set pvList ""
    set varList ""
    foreach name $nmList {
	if [regexp "PV" $name] {
	       lappend pvList $BPLD($name)
	       lappend varList BPLD($name.var)
	   }
    }
    
    set BPLD(pvList) $pvList
    set BPLD(varList) $varList
    set BPLD(faultVarList) {BPLD(IDfaultPV.var) BPLD(localMPSPV.var) BPLD(linkFaultPV.var) BPLD(main1fastPV.var)} 
    set BPLD(faultPVList) {BPLD(IDfaultPV) BPLD(localMPSPV) BPLD(linkFaultPV.var) BPLD(main1fastPV.var)}

    global errorCode
    #puts $pvList
    #flush
    set errorCode ""
    puts "step 0: connecting PVs..."
    if [pv linkw $varList $pvList 30] {
	puts stderr "Error connecting some pvs: $errorCode"
	exit 1
    }
    #setup values
    set BPLD(userRestPV.var) 1
    
    foreach ID {ID1 ID2} {
	foreach nm {H1 H2 V1 V2} bpm {X1 X2 Y1 Y2} {
	    #offset limit
	    set BPLD($ID.diamond${nm}LimitPV.var) 1000
	}
	foreach plane {H V} {
	    #offset limit
	    set BPLD($ID.rect${plane}OffsetLimitPV.var) 1000
	    #angle limit
	    set BPLD($ID.rect${plane}AgnleLimitPV.var) 250
	}
    }

    #main1 and main2 fast fault
    #local sector fast fault = [expr ($sector+1)/2]
    set fsector [expr ($sector+1)/2]
    #main2 fast and slow has 21 bits when there is fault
    set BPLD(findex) [expr 21 - $fsector]
    #main1 and main2 fast and slow faults findex must be 1
    
    puts "Step 0a: before start validation, check the local MPS fault, make sure it is zero. if not, fix it first!"
    if [catch {ClearFaults} fault] {
	return -code error "Error clearing faults: $fault"
    }
    if $fault {
	puts "Trying to clear the faults for second time..."
	if [catch {ClearFaults} fault] {
	    return -code error "Error clearing faults for second time: $fault"
	}
    }

     set BPLDNumber $BPLD(BPLDNumber)
    set BPLD(userResetPV.var) 1
    
    set BPLD(userResetPV.var) 1
    pv putw BPLD(userResetPV.var) 30
    after 2000
    puts "Step 0: before start validation, check the local MPS fault, make sure it is zero. if not, fix it first!"
    pv getw BPLD(localMPSPV.var) 30
    update
    set fault $BPLD(localMPSPV.var)
    puts "$BPLD(localMPSPV) $fault"
    if $fault {
	puts "The local MPS fault for S$BPLD(BPLDNumber) has to be cleared before starting validation!"
	exit 1
    }
    
    puts "To enable BM/ID validation, it needs following conditions"
    puts "1)the simulated hardware limit should be equal or greater than BM engaged current."
    #puts "2) software limit should be less than the hardware limit but greater than BM/ID engaged current."
    puts "2) MPS arming current should be greater than 0 (normally set to 0.5mA)"
 
    #disable BM, ID1, ID2, enable gap sim, and close gap
    set BPLD(ID1.enablePV.var) 1
    set BPLD(ID2.enablePV.var) 1
  
    set varList {BPLD(ID1.enablePV.var) BPLD(ID2.enablePV.var)}
    pv putw $varList 30
    pv putw BPLD(userResetPV.var) 30
    puts "reset and enable ID"
   
    update
    puts "S$BPLD(BPLDNumber) ID is ready for validation."
}

proc ClearFaults {args} {
    global BPLD
    if {0} {
	puts "set ID and BPM offsets to 0..."
	if [catch {APScavput -list=S$BPLD(BPLDNumber) \
		       -list=-LMPS:BPLD:ID -list=1,2 -list=:AIOR- \
		       -list=H,V -list=1,2 -list=-SP=0 -pend=60} result] {
	    return -code error "Error set ID bpm offsts to zero: $result"
	}
	
	#clear External inputs faults
	if [catch {APScavput -list=S$BPLD(BPLDNumber) -list=-LMPS:CtrlReg0-Cmd=0 -pend=30 } result] {
	    return -code error "Error clear ext inputs: $result"
	}
    }
   
    after 5000
    if [catch {APScavput -list=$BPLD(userResetPV)=1 -pend=30 } result] {
	return -code error "Error reset to clear faults: $result"
    }
    after 2000
    puts "check if faults are cleared..."
    if [catch {APScavget -list=$BPLD(localMPSPV) -printErrors -pend=30} fault] {
	return -code error "Error reading local MPS fault: $fault"
    }
    return $fault
}

proc LogIDValidationResult {args} {
    global BPLD
    
    if ![file exist $BPLD(logFile)] {
	set BPLD(logID) [open $BPLD(logFile) a+]
	puts $BPLD(logID) "SDDS1"
	puts $BPLD(logID) "&column name=StartTime, type=string, &end"
	puts $BPLD(logID) "&column name=EndTime, type=string, &end"
	puts $BPLD(logID) "&column name=Sector, type=string, &end"
	puts $BPLD(logID) "&column name=IDType, type=string, &end"
	puts $BPLD(logID) "&column name=BPM, type=string &end"
	puts $BPLD(logID) "&column name=WindowShape, type=string, &end"
	puts $BPLD(logID) "&column name=Plane, type=string, &end"
	puts $BPLD(logID) "&column name=ValidateType, type=string, &end"
	puts $BPLD(logID) "&column name=ValidationResult, type=string, &end"
	puts $BPLD(logID) "&column name=FaultDescription, type=string,&end"
	puts $BPLD(logID) "&data mode=ascii, no_row_counts=1, &end"
	exec chmod 777 $BPLD(logFile)
    } else {
	set BPLD(logID) [open $BPLD(logFile) a+]
    }
    switch $BPLD(window) {
	Diamond {
	    set shape Diamond
	}
	Rect {
	    set shape Rectangle
	}
	Both {
	    set shape Diamond-Rect
	}
    }
    set ID $BPLD(IDchoice)
    set valList $BPLD(startTime)
    lappend valList $BPLD(endTime)
    lappend valList S[format %02d $BPLD(sector)]
    set ID1 ID[format %02d [expr  $BPLD(sector)-1]]
    set ID2 ID[format %02d $BPLD(sector)]
    switch $BPLD(IDchoice) {
	ID1 {
	    lappend valList $ID1
	}
	ID2 {
	    lappend valList $ID2
	}
	Both {
	    lappend valList \"$ID1 $ID2\"
	}
    }
    
   # lappend valList $BPLD(IDchoice)
    switch $BPLD(IDchoice) {
	Both {
	    lappend valList \"$BPLD(ID1.useBPMs) $BPLD(ID2.useBPMs)\"
	}
	default {
	    lappend valList \"$BPLD($BPLD(IDchoice).useBPMs)\"
	}
    }
    lappend valList $shape
    lappend valList $BPLD(plane)
    lappend valList $BPLD(testType)
    lappend valList $BPLD(result)
    lappend valList \"$BPLD(faultDesc)\"
    puts $BPLD(logID) "[join $valList]"
    flush $BPLD(logID)
   # close $BPLD(logID)
}

proc ResetIDBPMOffsets {args} {
    global BPLD 

    set BPLDNumber $BPLD(BPLDNumber)
    if [catch {APScavput -list=S$BPLD(BPLDNumber) -list=-LMPS:BPLD: -list=ID1,ID2 -list=:AIOR- -list=H,V -list=1,2 -list=-SP=0 -pend=30} result] {
	return -code error "Error reset ID bpm offsets: $result"
    }
    
    set BPLD(userResetPV.var) 1
    pv putw BPLD(userResetPV.var) 30
}


proc CheckIDFaultStatus {args} {
    set fastAbort 0
    APSParseArguments {fastAbort}
    
    global BPLD
    set BPLDNumber $BPLD(BPLDNumber)
    SetDPBPLDStatus "checking ID bpm fault status..."
    
    if [catch {APScavget -list=$BPLD(IDfaultPV),$BPLD(localMPSPV),$BPLD(linkFaultPV),$BPLD(main1fastPV),$BPLD(main2fastPV),$BPLD(ID1faultPV),$BPLD(ID2faultPV) \
		   -printErrors -pend=30} valList] {
	return -code error "Error reading fault pvs: $valList"
    }
    set BPLD(faultDesc) ""
    set BPLD(IDfaultPV.var) [lindex $valList 0]
    set BPLD(localMPSPV.var) [lindex $valList 1]
    set BPLD(linkFaultPV.var) [lindex $valList 2]
    set BPLD(main1fastPV.var) [lindex $valList 3]
    set BPLD(main2fastPV.var) [lindex $valList 4]
    set BPLD(ID1faultPV.var) [lindex $valList 5]
    set BPLD(ID2faultPV.var) [lindex $valList 6]
    
    switch $BPLD(IDchoice) {
	ID1 {
	    if !$BPLD(ID1faultPV.var) {
		append BPLD(faultDesc) "ID1 fault not prodetected"
		return failed
	    } 
	}
	ID2 {
	    if !$BPLD(ID2faultPV.var) {
		append BPLD(faultDesc) "ID2 fault not detected"
		return failed
	    }
	}
	Both {
	    if !$BPLD(ID1faultPV.var) {
		append BPLD(faultDesc) "ID1 fault not detected"
		return failed
	    }
	    if !$BPLD(ID2faultPV.var) {
		append BPLD(faultDesc) "ID2 fault not detected"
		return failed
	    }
	}
    }
    if {$BPLD(IDfaultPV.var)==0} {
	append BPLD(faultDesc) "ID fault not detected!"
	return failed
    }
    #local MPS, ID fault=2
    if {$BPLD(localMPSPV.var)!=2} {
	append BPLD(faultDesc) "Local MPS ID fault not detected!"
	return failed
    }
    #link fault
    if {$BPLD(linkFaultPV.var)!=2} {
	append BPLD(faultDesc) "Local MPS ID link fault not detected!"
	return failed
    }
    
    if {!$BPLD(main1fastPV.var)} {
	append BPLD(faultDesc) "Main1 fast fault failed"
	return failed
    }
    if {!$BPLD(main2fastPV.var)} {
	append BPLD(faultDesc) "Main2 fast fault failed"
	return failed
    }
    return passed
}


proc CreateIDBumps {args} {
    set coord ""
    set validate 0
    APSParseArguments {coord valiate}
    global BPLD
    puts "1, $coord"
    switch $BPLD(plane) {
	both {
	    #only one plane is allowed for validation, so this case should never happen
	    set coordList {x y}
	    puts stderr "Error: ID BPLD validation can only do one plane at one time."
	    exit 1
	}
	default {
	    set coordList $BPLD(plane)
	}
    }
    set window $BPLD(window)
    set BPLDNumber $BPLD(BPLDNumber)
    switch $coord {
	x {
	    set plane H
	}
	y {
	    set plane V
	    #y plane does not have Both window,
	}
    }
    set BPLD(startTime) [clock format [clock  seconds] -format %Y%m%d-%H:%M:%S]
    set BPLD(faultDesc) ""
    puts $coord
    if $BPLD(validate) {
	SetDPBPLDStatus "Start ID bpm validation with offset..."
	SetDPBPLDStatus "clear faults..."
	if [catch {SetupValidation -type ID} result] {
	    return -code error "Error setup ID validation: $result"
	}
	SetDPBPLDStatus "Set BPLD ID BPM window shape..."
	if [catch {APScavput -list=S${BPLDNumber}-LMPS:BPLD:ID-Shape-SP=$BPLD(window) -pend=30} result] {
	    return -code error "Error setting ID bpm window shape..."
	}
	
	SetDPBPLDStatus "enabling ID..."
	if [catch {APScavput -list=S${BPLDNumber}-LMPS:BPLD:ID1_Enable-SP=1,S${BPLDNumber}-LMPS:BPLD:ID2_Enable-SP=1 \
		       -pend=30} result] {
	    return -code error "Error enable ID check: $result"
	}
    }
    set result0 passed
    set Coord [string toupper $coord]
    if {$BPLD(window)=="Rect" || $BPLD(window)=="Diamond"} {
	set points 4
	set start 2
    } else {
	set points 12
	set start 6
    }
    if $BPLD(fastAbort) {
	#fast abort only test one point
	set points 1
    }
    puts "Test points: $points"
   
    set win $BPLD(window)
    for {set j 1} {$j<=$points} {incr j} {
	SetDPBPLDStatus "zero setpoints..."
	if [catch {APScavput -list=[join $BPLD(bpmList) ,] -list=:$coord:SetpointC=0 -pend=10} result] {
	    return -code error "Error zero setpoint: $result"
	}
	if {$BPLD(testType)=="offset"} {
	    foreach ID $BPLD(IDList) {
		if [catch {APScavput -list=S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}1-SP=0,S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}2-SP=0 -pend=30} result] {
		    return -code error "Error setting offset: $result"
		}
	    }
	}
	after 10000
	puts "wait for orbit ..."
	if [catch {WaitForOrbit -coord $coord -limit $BPLD(tolerance) } result] {
	    return -code error "Error orbit after zero setpoint: $result"
	}
	if $BPLD(validate) {
	    set BPLD(userResetPV.var) 1
	    pv putw BPLD(userResetPV.var) 30
	    after 3000
	}
	set i $j
	if {$BPLD(method)==2} {
	    if {$BPLD(direction)==1} {
		set i $j
	    } elseif {$BPLD(direction)==-1} {
		if {$j<=[expr $points/2]} {
		    set i [expr $start + $j]
		} else {
		    set i [expr $j - $start]
		}
	    }
	}
	if $BPLD(fastAbort) {
	    set i $BPLD(fastAbortPoint)
	}
	SetDPBPLDStatus "working with contour point - $i..."
	foreach ID $BPLD(IDList) {
	    if {$BPLD(method)==3} {
		if {$ID=="ID1"} {
		    set i $j
		} else {
		    if {$j<=[expr $points/2]} {
			set i [expr $start + $j]
		    } else {
			set i [expr $j - $start]
		    }
		}
	    }
	    #reading virtual bpm readings
	    if [catch {APScavget -list=S$BPLD(sectorf)-LMPS:BPLD:${ID}_[string toupper $coord] \
			   -list=1,2 -list=-I -pend=10 -printErrors} valList] {
		return -code error "Error reading virtual BPM positions: $valList"
	    }
	    puts "2, $coord"
	    set s10 [lindex $valList 0]
	    set s20 [lindex $valList 1]
	    puts "working on ID: $ID"
	    set s0 [set BPLD($ID.ID$win.S0.${i}$coord)]
	    set sp0 [set BPLD($ID.ID$win.S0.${i}${coord}')]
	    
	    puts "$s0 $sp0  kkk"
	    #for any P0 and P1 combination, use L1 and L2 instead of L
	    
	    set s1 [expr 1.0e3 * ($s0 - abs($BPLD($ID.L1)) * $sp0)*1.15]
	    set s2 [expr 1.0e3 * ($s0 + $BPLD($ID.L2)  * $sp0)*1.15]
	    puts "computed S1/S2 $s1 $s2; current S1/S2 $s10, $s20"
	    #offset sign is different from s1 and s2
	    set offset1 [expr -1.0*$s1 + $s10]
	    set offset2 [expr -1.0*$s2 + $s20]    

	    puts  "ID=$ID setpoint1=$offset1 ($BPLD($ID.bpm1), setpoint2=$offset2 ($BPLD($ID.bpm2))"
	  
	    if {$BPLD(testType)=="offset"} {
		#use fireware offset PVs to create faults
		if [catch {APScavput -list=S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}1-SP=$offset1,S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}2-SP=$offset2 -pend=30} result] {
		    return -code error "Error setting offset: $result"
		}
		puts "set BPLD-offset cavput -list=S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}1-SP=$offset1,S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}2-SP=$offset2"
	    } else {
		puts " caput $BPLD($ID.bpm1):$coord:SetpointC $offset1"
		puts " caput $BPLD($ID.bpm2):$coord:SetpointC $offset2"
		if [catch {exec caput  $BPLD($ID.bpm1):$coord:SetpointC $offset1
		    exec caput $BPLD($ID.bpm2):$coord:SetpointC $offset2 } result] {
		    return -code error "Error setting setpoint: $result"
		}
	    } 
	}
	if $BPLD(validate) {
	    set BPLD(userResetPV.var) 1
	    pv putw BPLD(userResetPV.var) 30
	}
	after 10000
	puts "wait for orbit to converge..."
	if [catch {WaitForOrbit -coord $coord } result] {
	    return -code error "Error orbit after change setpoint: $result"
	}
	if $BPLD(fastAbort) {
	    #fast abort only run one point
	    set result [CheckIDFaultStatus]
	    return
	}
	after 3000
	if $BPLD(validate) {
	    puts "check trip state..."
	    set result [CheckIDFaultStatus]
	    if {0} {
		#just checking the pending state
		set result passed
		set BPLD(faultDesc) ""
		foreach ID $BPLD(IDList) {
		    if [catch {exec cavget -num -list=S$BPLD(sectorf)-BPLD:${ID}:$BPLD(coord)SumTripPendingM -pend=10 } value] {
			return -code error "Error reading trip pending state: $result"
		    }
		    if !$value {
			append BPLD(faultDesc) "$ID pending trip test failed."
			SetDPBPLDStatus "$ID pending trip test at ${i}the point failed"
			set result failed
		    }
		}
	    }
	    if {$result=="failed"} {
		set result0 failed
		SetDPBPLDStatus "ID offset validation at ${i}th point failed."
		break
	    } else {
		SetDPBPLDStatus "ID offset validation at ${i}th point passed."
	    }
	    set BPLD($ID.$coord.result) $result
	}
    }
    puts "3, $coord"
    set BPLD(endTime)  [clock format [clock  seconds] -format %Y%m%d-%H:%M:%S]
    set BPLD($BPLD(IDchoice).${coord}.result) $result0
    set BPLD(result) $result0
    puts "log result..."
    LogIDValidationResult  -type orbit
    if $BPLD(reset) {
	SetDPBPLDStatus "reset setpoints to zero..."
	if [catch {APScavput -list=[join $BPLD(bpmList) ,] -list=:$coord:SetpointC=0 -pend=10} result] {
	    return -code error "Error zero setpoint: $result"
	}
	if {$BPLD(testType)=="offset"} {
	    foreach ID $BPLD(IDList) {
		if [catch {APScavput -list=S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}1-SP=0,S$BPLD(BPLDNumber)-LMPS:BPLD:${ID}:AIOR-${plane}2-SP=0 -pend=30} result] {
		    return -code error "Error setting offset: $result"
		}
	    }
	}
	puts "wait for orbit to converge..."	
	if [catch {WaitForOrbit -coord $coord } result] {	
	    return -code error "Error orbit after change setpoint: $result"
	}
	after 3000
    }
    if $BPLD(validate) {
	#clear faults
	#set BPLD(userResetPV.var) 1
	#pv putw BPLD(userResetPV.var) 30
    }
    puts "done."
}

proc WaitForOrbit {args} {
    set limit 100
    set timeout 300
    set coord x
    APSParseArguments {limit timeout coord}
    global BPLD
    set timeout [expr [clock seconds]+$timeout]
    set first 1
    if $BPLD(noOrbitCheck) {
	#no real orbit check in simulation mode
	return
    }
    while {[clock seconds]<$timeout} {
	if $BPLD(fastAbort) {
	    #return if beam is dumped
	    if [catch {APScavget -list=S-DCCT:CurrentM -pend=10 -printError} srCurrent] {
		return -code error "Error reading SR current: $srCurrent"
	    }
	    if {$srCurrent<0.05} {
		puts "beam is dumped!"
		if [catch {APScavput -list=[join $BPLD(bpmList) ,] \
			       -list=:$coord:SetpointC=0 -pend=10} result] {
		    return -code error "Error zero setpoint: $result"
		}
		return
	    }
	}
	if [catch {APScavget -list=[join $BPLD(bpmList) ,] -list=:$coord:SampledErrorM -pend=10 } valList] {
	    return -code error "Error reading orbit: $valList"
	}
	set done 1
	if $first {
	    puts "cavget -list=[join $BPLD(bpmList) ,] -list=:$coord:SampledErrorM"
	    set first 0
	}
	foreach val $valList pv $BPLD(bpmList) {
	    if {[expr abs($val)]>$limit} {
		set done 0
		break
	    }
	}
	if $done {
	    break
	}
	after 1000
    }
    if !$done {
	return -code error "Error: orbit not converged!"
    }
}
puts "coord: $BPLD(coord)"
catch {CreateIDBumps -coord $BPLD(coord) } result
puts $result
puts "restore setpoints..."
if [catch {exec cavput -list=[join $restoreList ,] -pend=10} result] {
    puts stderr "Error restore setpoints: $result"
}

exit 0

