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

#create matrix with steered (local) BPMs and (local) correctors only.
#makeLocalSteeringMatrices should be run before this script is called.
#i.e assume the full matrix of irm, irm.x and irm.y have been created and up to date.
#

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)]
APSDebugPath
APSStandardSetup
set usage "makeLocalSteeringMatricesNew -lattice <latticeDir> -steeringType {CUUSP0|CUDSP0|ID|BM} -sourceList <list of sectors>\n"
set args $argv
set lattice default
set steeringType ""
set sourceList ""
APSParseArguments {lattice steeringType sourceList}
if {![string length $lattice] || ![string length $steeringType]} {
    puts stderr $usage
    exit
}

proc MakeSectorMatrix {args} {
    set sector ""
    set steeringType ""
    APSParseArguments {sector steeringType}
    
    global refLatticeDir targetDir twissFile
    
    set controllawDir $targetDir/${steeringType}s/[format %02ld ${sector}]$steeringType
    set tmpRoot /tmp/[APSTmpString]
    set p1s 11.0
    set p2s 18.0
    set S0 0
    if {$steeringType=="BM"} {
        if [catch {exec sddsprocess $twissFile -pipe=out -match=col,ElementName=S${sector}B:M \
                     | sddsprocess -pipe -process=s,first,BMsourceStart \
                     | sdds2stream -pipe -par=BMsourceStart } S0] {
            return -code "Error obtaining BM source start point of $sector sector: $S0"
        }
    } else {
        if [catch {exec sddsprocess /home/helios/oagData/sr/IDs/sectors.sdds -filter=col,Sector,$sector,$sector \
                     $tmpRoot.$sector.distance } result] {
            #return -code error "MakeSectorMatrix0: $result"
            #not exist in the xray bpm files
            set p1distance 16.35
            set p2distance 20.09
        } else {
            set p1distance [exec sdds2stream -col=P1Distance $tmpRoot.$sector.distance]
            set p2distance [exec sdds2stream -col=P2Distance $tmpRoot.$sector.distance]
        }
    }
    
    foreach Plane {H V} plane {h v} coord {x y} {
        if [catch {exec sddsprocess $controllawDir/irm -pipe=out -match=col,ControlName=*${Plane}* \
                     | sddsconvert -pipe -retain=col,ControlName \
                     | sddsxref -pipe $twissFile -match=ControlName=ElementName -take=s -nowarnings \
                     | sddsprocess -pipe=in $tmpRoot.$plane.1 -process=s,first,s0 -process=s,last,s1 } result] {
            return -code error "MakeSectorMatrix1: $result"
        }
        set s0 [exec sdds2stream -par=s0 $tmpRoot.$plane.1]
        set s1 [exec sdds2stream -par=s1 $tmpRoot.$plane.1]
        set corrList [exec sdds2stream -col=ControlName $tmpRoot.$plane.1]
        #create corrector range error file
        set corrNameList ""
        set corrValueList ""
        foreach corr $corrList {
            lappend corrNameList ${corr}:RangeErrorCALC.HIHI
            lappend corrValueList 50
            lappend corrNameList ${corr}:RangeErrorCALC.LOLO
            lappend corrValueList -50
        }
        if [catch {exec sddsmakedataset -col=CorrName,type=string -data=[join $corrList ,] $controllawDir/$plane.corrName } result] {
            return -code error $result
        }

        if [catch {exec sddsmakedataset -pipe=out -col=ControlName,type=string -data=[join $corrNameList ,] \
                     -col=ValueString,type=string -data=[join $corrValueList ,] \
                     | tee $controllawDir/$plane.corr.range \
                     | sddscasr -pipe=in -save $controllawDir/$plane.corr.range.orig } result] {
            return -code error "Error getting corrector rangeError values: $result"
        }

        set ${plane}corrList($sector) $corrList
       
        if [catch {exec sddsprocess $twissFile -pipe=out -filter=col,s,$s0,$s1 -match=col,ElementType=MONI \
                     | tee $tmpRoot.$sector.bpms \
                     | sdds2stream -pipe -col=ElementName } bpmList] {
            return -code error "MakeSectorMatrix2: error getting in bound bpms for $sector: $bpmList"
        }
        set sList [exec sdds2stream -col=s $tmpRoot.$sector.bpms]
        set equationCmd ""
        set xrayBPMList ""
        set xrayBPMEqnList ""
        set i1 -1
        set i2 -1
        set p1 ""
        set p2 ""
        switch $steeringType {
            BM {
                if {$plane=="v"} {
                    set xrayBPMList [APSGetBMXRayBPMList -plane $plane]
                    set xrayBPMEqnList [APSGetBMXRayEqnList -plane $plane]
                    set p1 S${sector}BM:P1
                    set p2 S${sector}BM:P2
                }
            }
            default {
                set xrayBPMList [APSGetIDXRayBPMList -plane $plane]
                set xrayBPMEqnList [APSGetIDXRayEqnList -plane $plane]
                if {$steeringType=="ID"} {
                    set p1 S${sector}ID:P1
                    set p2 S${sector}ID:P2
                } elseif {$steeringType=="CUUSP0"} {
                    set p1 S${sector}ID:P1
                    set p2 ""
                } elseif {$steeringType=="CUDSP0"} {
                    set p1 ""
                    set p2 S${sector}ID:P2
                }
            }            

        }
        if [string length $p1] {
            set i1 [lsearch -exact $xrayBPMList $p1]
        }
        if [string length $p2] {
            set i2 [lsearch -exact $xrayBPMList $p2]
        }
        if {$i1>=0} {
            if ![string length $equationCmd] {
                lappend equationCmd sddsprocess -pipe
            }
            lappend bpmList $p1
            set eq1 [lindex $xrayBPMEqnList $i1]
            lappend equationCmd "-define=column,$p1,$eq1"
            if {$steeringType=="BM"} {
                lappend sList [expr $S0 + $p1s]
            } else {
                lappend sList [expr $sector * 27.6 + $p1distance]
            }
        }
        if {$i2>=0} {
            if ![string length $equationCmd] {
                lappend equationCmd sddsprocess -pipe
            }
            lappend bpmList $p2
            set eq2 [lindex $xrayBPMEqnList $i2]
            lappend equationCmd "-define=column,$p2,$eq2"
            if {$steeringType=="BM"} {
                lappend sList [expr $S0 + $p2s]
            } else {
                lappend sList [expr $sector * 27.6 + $p2distance]
            }
        }
        set refMatrix $refLatticeDir/${plane}.default
        
        set opt ""
        set pvList ""
        set pvList1 ""
        if [catch {exec sddsmakedataset -col=BPMName,type=string -data=[join $bpmList ,] \
                     -col=s,type=double -data=[join $sList ,] \
                     $tmpRoot.$sector.s } result] {
             return -code error "Making sector matrix3a: $result"
        }
        foreach bpm $bpmList {
            lappend pvList ${bpm}:ms:${coord}:SetpointAO
            lappend pvList1 ${bpm}:ms:${coord}:SetpointAO
            if ![string length $opt] {
                set opt -match=col,BPMName=$bpm
            } else {
                append opt ,BPMName=$bpm,|
            }
        }
        if [string length $equationCmd] {
            if [catch {eval exec sddstranspose $refMatrix -pipe=out \
                         | $equationCmd \
                         | sddstranspose -pipe \
                         -newColumnNames=OldColumnNames -oldColumnNames=BPMName \
                         | sddsprocess -pipe $opt \
                         | sddsconvert -pipe -del=col,s \
                         | sddsxref -pipe $tmpRoot.$sector.s -match=BPMName -nowarnings -take=s \
                         | sddssort -pipe -col=s \
                         | sddsconvert -pipe=in -retain=col,BPMName,[join $corrList ,] \
                         $controllawDir/$plane.steering.rm } result] {
                return -code error "Making sector matrix3a: $result"
            }
        } else {
            if [catch {eval exec sddsprocess $refMatrix -pipe=out $opt \
                         | sddsconvert -pipe -del=col,s \
                         | sddsxref -pipe $tmpRoot.$sector.s -match=BPMName -nowarnings -take=s \
                         | sddssort -pipe -col=s \
                         | sddsconvert -pipe=in -retain=col,BPMName,[join $corrList ,] \
                         $controllawDir/$plane.steering.rm } result] {
                return -code error "Making sector matrix3b: $result"
            }
        }
    }
    set tmpFiles [glob $tmpRoot.*]
    APSAddToTmpFileList -ID steering -fileList $tmpFiles
}

set refLatticeDir /home/helios/oagData/sr/orbitControllaw/lattices/${lattice}/refMatrices
set configDir /home/helios/oagData/sr/localSteering/configs
set targetDir /home/helios/oagData/sr/localSteering/lattices/${lattice}
set twissFile /home/helios/oagData/sr/lattices/$lattice/aps.twi 


# For canted undulator configurations. These configuration use P0 steering.
set CUdataDir /home/helios/oagData/sr/XAXSTF
if [catch {exec sddsprocess $CUdataDir/sectors.sdds -pipe=out \
             -filter=col,CUflag,1,1 \
             | sdds2stream -pipe -col=Sector} CUlist] {
    return -code error "$CUlist"
}
cd $targetDir
switch -regexp $steeringType {
    CUUSP0|CUDSP0|ID|BM {
        if ![file exists ${steeringType}s] {
            puts stderr "You need run makeLocalSteeringMatrices first."
            exit 0
        }
    }
    default {
        puts stderr "makeLocalSteeringMatricesNew: Invalid source type: $steeringType\n$usage"
        exit
    }
}

if ![string length $sourceList] {
    switch $steeringType {
        ID {
            set sourceList [exec sdds2stream -col=Sector /home/helios/oagData/sr/IDs/sectors.sdds]
        }
        BM {
            set sourceList [exec sdds2stream -col=Sector /home/helios/oagData/sr/BMs/sectors.sdds]
        }
        default {
            #for CUUSP0|CUDSP0
            set sourceList $CUlist
        }
    }
}

foreach sector $sourceList {
    if [catch {MakeSectorMatrix -sector $sector -steeringType $steeringType } result] {
        puts stderr "Error in making $steeringType matrix for sector $sector: $result"
        exit 1
    }
}

exit

# Local Variables:
# mode: tcl
# indent-tabs-mode: nil
# End:
