#!/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)]
APSDebugPath
APSStandardSetup
set usage "makeBoosterLocalSteeringMatrices -lattice <latticeDir>"

proc MakeSectorMatrix {args} {
    set sector ""
    APSParseArguments {sector}
    global $sector  steeringDir  refMatrixDir   twissFile 

    foreach plane {h v} coord {x y} {
        set Plane [string toupper $plane]
        if {$sector=="B2C9" && $plane=="v"} {
            continue
        }
        set corr1 [set ${sector}($plane.corr1)]$Plane
        set corr2 [set ${sector}($plane.corr2)]$Plane
        set corr3 [set ${sector}($plane.corr3)]$Plane
        set corr4 [set ${sector}($plane.corr4)]$Plane
        set bpm [set ${sector}($plane.BPMName)]
        set pollutedBPMs [set ${sector}($plane.pollutedBPMs)]
        
        puts "$sector $plane $bpm $pollutedBPMs"
        set opt -match=col,BPMName=$bpm
        foreach bpm $pollutedBPMs {
            if ![string length $opt] {
                set opt -match=col,BPMName=$bpm
            } else {
                append opt ,BPMName=$bpm,|
            }
        }
        if {$plane=="h"} {
            #use all bpms due to the dispersion
            if [catch {exec sddsconvert $refMatrixDir/${plane}.default -retain=col,BPMName,$corr1,$corr2,$corr4 \
                         $steeringDir/$sector/$plane.steering.rm } result] {
                return -code error "MakeSectorMatrix1: $result"
            }
        } else {
            #for vertical plane, only steer bpm and polluted bpms are needed, all other bpms are zero
            #because the fixed length and free lenght matrices are the same (no dispersion on y plane)
            if [catch {eval exec sddsprocess $refMatrixDir/${plane}.default -pipe=out $opt \
                         | sddsconvert -pipe=in -retain=col,BPMName,$corr1,$corr2,$corr4 \
                         $steeringDir/$sector/$plane.steering.rm } result] {
                return -code error "MakeSectorMatrix6: $result"
            }
        }
    }
}

proc ReadBoosterSteeringConfig {args} {
    global sectorList
    eval global $sectorList
    
    set configFile /home/helios/oagData/booster/localSteering/steeringConfig.sdds
   # foreach nm {Sector BPMName Corr1 Corr2 Corr3 Corr4 PollutedBPMs} {
   #     set ${nm}List [exec sdds2stream -col=$nm $configFile]
   # }
  #  set sectorList $SectorList
    set index 0
    #corr3 will not be used in steering
    foreach plane {h v} {
        set configFile /home/helios/oagData/booster/localSteering/${plane}steeringConfig.sdds
        foreach nm {Sector BPMName Corr1 Corr2 Corr3 Corr4 PollutedBPMs} {
            set ${nm}List [exec sdds2stream -col=$nm $configFile]
        }
        set sectorList $SectorList
        set index 0
        foreach sector $SectorList {
            set ${sector}($plane.BPMName) [lindex $BPMNameList $index]
            set ${sector}($plane.corr1) [lindex $Corr1List $index]
            set ${sector}($plane.corr2) [lindex $Corr2List $index]
            set ${sector}($plane.corr3) [lindex $Corr3List $index]
            set ${sector}($plane.corr4) [lindex $Corr4List $index]
            set ${sector}($plane.pollutedBPMs) [lindex $PollutedBPMsList $index]
            incr index
        }
    }
}

set args $argv
set lattice default

APSStrictParseArguments {lattice}

if ![string length $lattice] {
    puts stderr $usage
    exit
}

set twissFile /home/helios/oagData/booster/localSteering//lattices/$lattice/refMatrices/booster.twi
#set refMatrixDir /home/helios/oagData/booster/orbitControllaw/lattices/${lattice}/refMatrices
set refMatrixDir /home/helios/oagData/booster/lattices/default/refMatrices
#h.default is the fixed length matrix, h.free.default is the free length matrix 
#use free matrix to obtaint the corrector change
#then use fixed length matrix to get the bpm setpoint change

set steeringDir /home/helios/oagData/booster/localSteering/lattices/$lattice
set configFile /home/helios/oagData/booster/localSteering/steeringConfig.sdds

set sectorList ""
for {set B 1} {$B<=4} {incr B} {
    for {set C 0} {$C<=9} {incr C} {
        lappend sectorList B${B}C${C}
    }
}

if [catch {ReadBoosterSteeringConfig} result] {
    puts stderr "Error1: $result"
    exit 1
}

cd $steeringDir
set tmpRoot /tmp/[APSTmpString]
foreach s $sectorList {
    
    set subDir $s
    puts "working on sector $s..."
    if ![file exists $subDir] {
        if [catch {exec mkdir -p $subDir} result ] {
            puts stderr "makeBoosterLocalSteeringMatrices1: $result"
            exit
        }
    }
    # clean subdirectory
    catch {eval file delete [glob -nocomplain $subDir/*]}
    foreach plane {h v} {
        switch $plane {
            h {
                set coord x
                set otherPlane V
            }
            v {
                set coord y
                set otherPlane H
            }
            default {}
        }
        if {$s=="B2C9" && $plane=="v"} {
            continue
        }
        set Plane [string toupper $plane]
        set bpm ${s}($plane.BPMName)
        set pollutedBPMs [set ${s}($plane.pollutedBPMs)]
        set matchOpt ""
        foreach bpm $pollutedBPMs {
            lappend matchOpt "-match=col,BPMName=$bpm,!"
        }
        set corr1 [set ${s}($plane.corr1)]$Plane
        set corr2 [set ${s}($plane.corr2)]$Plane
       # set corr3 [set ${s}(corr3)]  not use
        set corr4 [set ${s}($plane.corr4)]$Plane
        if [catch {eval exec sddsconvert $refMatrixDir/${plane}.free.default -pipe=out -retain=col,BPMName,$corr1,$corr2,$corr4 \
                     | sddsprocess -pipe $matchOpt \
                     | tee $subDir/rm.$coord \
                     | sddspseudoinverse -pipe=in $subDir/irm.$coord  \
                     -largestSingularValues=3 \
                     -oldColumnNames=ControlName \
                     -vmatrix=$subDir/v.${coord} \
                     -umatrix=$subDir/u.${coord} \
                     -largestSingularValues=4 } result ] {
            return -code error "makeBoosterLocalSteeringMatrices(2): $result"
        }
        if [catch {exec sdds2stream $subDir/irm.${coord} \
                     -para=ConditionNumber} ConditionNumber($plane)] {
            return -code error "makeLocalSteeringMatrices(2a): $result"
        }
    }
    
    #make new steering matrices here
    if [catch {MakeSectorMatrix -sector $s } result] {
        puts stderr "Error in making steering matrix for sector $s: $result"
        exit 1
    }
}

puts "done."

exit 0
