#!/bin/sh
# \
exec oagwish "$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 CVSRevisionAuthor "\Revision 1.0 $ \$Author: Shang $"
set appName "FBC Response Matrix measurement (using All AFGs)"

set env(EPICS_CA_ADDR_LIST) "ctlsdaqsrv1 iocrtfbdaq ctlsdaqdev2 ctlsdaqdev1.aps4.anl.gov"
set env(EPICS_CA_MAX_ARRAY_BYTES) 1048576

set refDir /home/helios/oagData/sr/rtfeedback/monitorFiles
set modelDir /home/helios/oagData/sr/rtfeedback/lattices/default

set HModelDir h.2018-0523.00
set VModelDir v.2018-0523.00

#set outputDir /home/helios/SR/daily/2017/06/19/2/S27IntegratedStudies/SineData
set outputDir /home/helios/SR/daily/2017/07/17/3/S27IntegratedStudies/Closed_Loop_RespMeasurement
set outputDir .
set outputDir /home/helios/SR/daily/2017/09/28/2/S27IntegratedStudies/integral
#set outputDir  /home/helios/SR/daily/2017/09/28/2/S27IntegratedStudies/integral
#set outputDir /home/helios/SR/daily/2017/10/09/3/S27IntegratedStudiess
#set outputDir /home/helios/SR/daily/2018/05/23/3/S27IntegratedStudies

set corr S27A:H3
#set fileName ${corr}_Sine_44Hz_5Amp
set CurrentAmp 2.5
set interval 1.2
set steps 10
set plane H
set status ""
set alpha 0.1
set freq 44
set monDir /home/helios/oagData/sr/rtfeedback/monitorFiles

#new data collected at
#/home/helios/SR/daily/2018/06/26/2/S27IntegratedStudies

proc SetStatus {text} {
    global status
    set status "[exec date] $text"
    update
}

set BPMList {S27A:P0 S27A:P2 S27A:P3 S27A:P4 S27B:P4 S27B:P3 S27B:P2 S27B:P0 S28A:P0 S28A:P2 S28A:P3 S28A:P4 S28B:P4 S28B:P3 S28B:P2 S28B:P0}

proc Start {args} {
    set takeDataOnly 0
    APSParseArguments {takeDataOnly}
    
    global outputDir plane modelDir  rootname  steps monDir HCurrentAmp VCurrentAmp
    global CorrH1Np CorrH2Np CorrH3Np CorrH4Np interval HModelDir VModelDir irmLoad
    global CorrV1Np CorrV2Np CorrV3Np CorrV4Np CorrH1Freq CorrH2Freq CorrH3Freq CorrH4Freq
    global CorrH1Freq CorrH2Freq CorrH3Freq CorrH4Freq
    global CorrV1Freq CorrV2Freq CorrV3Freq CorrV4Freq
    
    # load perturbation waveform once 
    set HcorrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4}
    set VcorrList {S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    set filename ${rootname}.sdds
    cd $outputDir
    if [file exist $filename] {
        if ![APSYesNoPopUp "$filename already exist, overwrite it?"] {
            SetStatus "$filename already exist, please provide new rootname to avoid overwrite it."
            return
        }
    }
    SetStatus "Load model IRM into FBC..."
    catch {exec mkdir -p $rootname}
    foreach plane {H V} {
        set dir $modelDir/[set ${plane}ModelDir]
        set irm $dir/${irmLoad}.mat.$plane
        puts $irm
        set rmModel [regsub {irm_} $irm ""]
        if ![file exist $irm] {
            set irm $dir/irm
            set rmModel $dir/rm
        }
        if [APSYesNoPopUp "Load $irm into FBC?"] {
            if [catch {LoadIRM -irmFile $irm -plane $plane} result] {
                SetStatus "Error loading $irm into FBC: $result"
                return
            }
            catch {exec rm $rootname/${plane}rm}
            exec ln -s $rmModel $rootname/${plane}rm
        } else {
            if ![APSYesNoPopUp "Would you like to continue measurement without loading $irm into FBC?"] {
                SetStatus "Measurement is cancelled."
                return
            }
        }
    }

    SetStatus "compute and load perturb frequency (AFG) waveform..."
    if [catch {LoadAFG -generate 1 -plot 0} result] {
        SetStatus "Error in updating perturb frequency waveform : $result"
        return
    }
    SetStatus "start measurement ..."
    
    set sampleRate [exec cavget -list=U:S27:OFB:DSP:LoopRateM -repeat=number=15,pause=0.3,average]
    SetStatus "Sample Rate for 15 averages = ${sampleRate} Hz"
    SetStatus "collecting data..."
    if [catch {exec sddswmonitor $monDir/FBCstepFile.wmon $filename -scalars=$monDir/FBC.mon \
                 -interval=$interval -steps=$steps -verbose -erase } result] {
        SetStatus "Error collecting data1: $result"
        return
    }
    if [catch {exec sddsmakedataset $filename.parameters \
                 -column=SampleRate,type=double -data=$sampleRate \
                 -col=HCurrentAmp,type=double -data=$HCurrentAmp \
                 -col=VCurrentAmp,type=double -data=$VCurrentAmp \
                 -col=CorrH1Freq,type=double -data=$CorrH1Freq \
                 -col=CorrH2Freq,type=double -data=$CorrH2Freq \
                 -col=CorrH3Freq,type=double -data=$CorrH3Freq \
                 -col=CorrH4Freq,type=double -data=$CorrH4Freq \
                 -col=CorrV1Freq,type=double -data=$CorrV1Freq \
                 -col=CorrV2Freq,type=double -data=$CorrV2Freq \
                 -col=CorrV3Freq,type=double -data=$CorrV3Freq \
                 -col=CorrV4Freq,type=double -data=$CorrV4Freq \
                 -col=CorrH1Np,type=long -data=$CorrH1Np \
                 -col=CorrH2Np,type=long -data=$CorrH2Np \
                 -col=CorrH3Np,type=long -data=$CorrH3Np \
                 -col=CorrH4Np,type=long -data=$CorrH4Np  \
                 -col=CorrV1Np,type=long -data=$CorrV1Np \
                 -col=CorrV2Np,type=long -data=$CorrV2Np \
                 -col=CorrV3Np,type=long -data=$CorrV3Np \
                 -col=CorrV4Np,type=long -data=$CorrV4Np } result] {
        return -code error "Error creating parameters: $result"
    }
    
    if $takeDataOnly {
        SetStatus "done collecting data."
        return
    }
    catch {exec rm ${filename}~}
    SetStatus "Processing data..."
    if [catch {ProcessData -rootname $rootname } result] {
        SetStatus "Error processing data: $result"
        return
    }
    
    SetStatus "Compute Matrix ..."
    if [catch {ComputeResponseMatrix -plot 1} result] {
        SetStatus "Error computing matrix: $result"
        return
    }
    SetStatus "$rootname matrix measurement is done"
}


proc LoadIRM {args} {
    set plot 0
    set load 1
    set irmFile ""
    set plane ""
    APSParseArguments {plot load irmFile plane}
    global modelDir outputDir S27A3 S27B4 S28A3 S28B4 HModelDir VModelDir rootname
    cd $outputDir
    if ![string length $plane] {
        set planeList {H V}
    } else {
        set planeList $plane
    }
    foreach plane $planeList {
        set pl [string tolower $plane]
        if ![string length $irmFile] {
            set dir $modelDir/[set ${plane}ModelDir]
            set IRM [APSFileSelectDialog .select -listDir $dir -pattern *irm*]
        } else {
            set IRM $irmFile
        }
        if ![file exist $IRM] {
            return -code error "Error: irm matrix $IRM does not exist!"
        }
        
        set irmPV  U:S27:OFB:${plane}:Irm16x8AC
        if [catch {exec sddswget prev_irm.$plane.sdds -pvname=$irmPV } result] {
            return -code error "Error reading $irmPV: $result"
        }
        set matrixFile irm.matrix.$plane
        set corrList [list S27A:${plane}2 S27A:${plane}3 S27B:${plane}4 S27B:${plane}2 S28A:${plane}2 S28A:${plane}3 S28B:${plane}4 S28B:${plane}2]
        if [catch {exec sddstranspose $IRM  -pipe=out \
                     | sddsprocess -pipe=in  $matrixFile  "-redefine=col,S27A:${plane}2,0" \
                     "-redefine=col,S27B:${plane}2,0" \
                     "-redefine=col,S28A:${plane}2,0" \
                     "-redefine=col,S28B:${plane}2,0" } result] {
            return -code error "Error processing matrix file: $result"
        }
        set fileList ""
        set tmpRoot /tmp/[APSTmpString]
        foreach corr $corrList {
            if [catch {exec sddsconvert $matrixFile -retain=col,$corr -pipe=out \
                         | sddsconvert -pipe=in $tmpRoot.$corr -rename=col,$corr=Waveform } result] {
                return -code error "Error $corr data not found: $result"
            }
            lappend fileList $tmpRoot.$corr
        }
        if [catch {eval exec sddscombine $fileList -merge -pipe=out \
                     | sddsprocess -pipe=in $matrixFile.load.$plane -redefine=col,Index,i_row,type=long \
                     "-redefine=col,Waveform,Waveform,type=float" \
                     -reprint=par,WaveformPV,$irmPV } result] {
            return -code error "Error generating waveform file: $result"
        }
    
        #LOAD 
        if $load {
            SetStatus "loading $plane irm..."
            if [catch {exec sddswput $matrixFile.load.$plane -pend=30} result] {
                return -code error "Error loading matrix into IOC: $result"
            }
            set desc1 "Previous FBC $plane IRM"
            set desc2 "Present FBC $plane IRM"
            SetStatus "done."
        } else {
            set desc1 "FBC $plane IRM"
            set desc2 "Selected $plane IRM"
        }
        if $plot {
            SetStatus "checking $plane plane..."
            exec sddsplot -grap=sym,conn,vary=sub  "-ylabel=Matrix element(mm/A)" -col=Index,Waveform "-leg=spec=$desc1" prev_irm.$plane.sdds \
              -col=Index,Waveform "-leg=spec=$desc2"  $matrixFile.load.$plane  &
        }
    }
    SetStatus "done."
}

proc ComputeFrequency {args} {
    set load 0
    APSParseArguments {load}
    global freq CurrentAmp outputDir
    set sampleRate [exec cavget -list=U:S27:OFB:DSP:LoopRateM ]
    
    global rootname outputDir
    
    set N 20480
    set time [expr $N * 1.0 / $sampleRate]
    set Np [expr int($freq * $time)]
   # puts $sampleRate
   # puts $Np
    set PI 3.141592653589793238462643
    set Np 40
    set freq [format %.3f [expr $Np * 1.0 /($N -1.0) * $sampleRate]]
   # set freq [format %.3f [expr 2.0 * $PI / 512.0]]
    if [catch {exec sddssequence -pipe=out -define=Index,i_row,type=long \
                 -sequence=begin=0,end=[expr $N-1],interval=1,delta=1 \
                 | sddsprocess -pipe=in  $outputDir/${rootname}-perturbWaveform.sdds  \
                 "-reprint=par,Description,Perturbation Waveform at $freq Hz" \
                 "-redefine=col,time,Index $sampleRate /,units=s" \
                 "-reprint=par,WaveformPV,U:S27:OFB:${AFG}:WaveformC" \
                 "-redefine=col,Waveform,2.0 $PI * Index * 512 / sin $CurrentAmp *,units=Amps" } result] {
        return -code error "computefrequency0: $result"
    }
    exec sddsplot -grap=sym -col=time,Waveform  $outputDir/${rootname}-perturbWaveform.sdds -topline=@Description &
    if [catch {exec sddsfft $outputDir/${rootname}-perturbWaveform.sdds -pipe=out -col=Index,Waveform -full \
                 | sddsprocess -pipe  \
                 "-reprint=par,Description,Perturbation FFT Waveform at $freq Hz." \
                 "-process=FFTWaveform,max,FFTWaveformMax" \
                 "-process=FFTWaveform,max,FFTPeakFreq,functionof=f,position" \
                 | sddsprocess -pipe=in  $outputDir/${rootname}-perturbWaveform.fft "-redefine=col,ImagFFTWaveformAbs,ImagFFTWaveform abs" \
                 "-process=ImagFFTWaveformAbs,max,ImagFFTWaveformAbsMax" \
                 "-process=ImagFFTWaveformAbs,max,ImagAbsPeakFreq,function=f,position" \
                 "-process=ImagFFTWaveformAbs,max,ImagPeak,functionof=ImagFFTWaveform,position" \
             } result] {
        return -code error "computefrequency1: $result"
    }
    exec sddsplot $outputDir/${rootname}-perturbWaveform.fft  -col=f,FFT*  -topline=@Description \
      -par=FFTPeakFreq,FFTWaveformMax -grap=sym,sub=1,scale=2 -leg=spec=FFTPeakFreq -par=ImagAbsPeakFreq,ImagFFTWaveformAbsMax -grap=sym,sub=2,scale=2 -leg=spec=ImagAbsPeakFreq   &
    set imagPeak [exec sdds2stream -par=ImagPeak $outputDir/${rootname}-perturbWaveform.fft]
    
    if $load {
        if [catch {LoadAFG} result] {
            return -code error "Error loading AFG: $result"
        }
    }
}


proc PlotRawData {args} {
    global rootname outputDir
    cd $outputDir
    set timeLimit 1.0
    if ![file exist $rootname.sdds.env] {
        exec sddsenvelope -pipe=out $rootname.sdds \
          -mean=Index,Time,U* \
          | sddsprocess -pipe=in $rootname.sdds.env  \
          -redefine=col,Time,TimeMean,units=s
    }
    foreach plane {H V} coord {X Y} {
        set corrList [list S27A:${plane}3 S27B:${plane}4 S28A:${plane}3 S28B:${plane}4]
        catch {exec rm rawCorrBPMErrorData1-$rootname.$plane.eps rawCorrBPMErrorData1-$rootname.$plane.png}

        exec sddsplot3 rawCorrBPMErrorData1-$rootname.$plane  $rootname.sdds.env -graph=line,vary -layout=1,6 -mode=y=offset \
          -limits=xMinimum=0,xMaximum=${timeLimit} \
          -leg=editCommand=%/Mean\[U://%/:OFB//%/Long//%/HistAM\]//%/:OFB:LongErrorHistAM\]//%/X:Error// \
          -col=Time,U*S27A:${plane}3*Drive* -leg=spec=S27A:H3:Drive  \
          -col=Time,U*S27B:${plane}4*Drive* -leg=spec=S27B:H4:Drive  \
          -col=Time,U*S28A:${plane}3*Drive* -leg=spec=S28A:H3:Drive  \
          -col=Time,U*S28B:${plane}4*Drive* -leg=spec=S28B:H4:Drive  \
          "-topline=Open Loop All Four Horizontal Correctors with AFG Waveform Applied" \
          "-yLabel=Current Change (Amps)" -endp \
          -col=Time,U*S27A:${plane}3*Error* -leg=spec=S27A:H3:Error \
          -col=Time,U*S27B:${plane}4*Error* -leg=spec=S27B:H4:Error \
          -col=Time,U*S28A:${plane}3*Error* -leg=spec=S28A:H3:Error \
          -col=Time,U*S28B:${plane}4*Error* -leg=spec=S28B:H4:Error \
          "-yLabel=Current Change (Amps)" -endp \
          -col=Time,U*S*P0*${coord}*  \
          "-yLabel=BPM Error (mm)" -endp \
          -col=Time,U*S*P2*${coord}*  \
          "-yLabel=BPM Error (mm)" -endp \
          -col=Time,U*S*P3*${coord}*  \
          "-yLabel=BPM Error (mm)" -endp \
          -col=Time,U*S*P4*${coord}*  \
          "-yLabel=BPM Error (mm)" \
          "-title=All Four Horizontal Fast Correctors Driven"  &
        
        after 3000
        
        catch {exec rm rawCorrBPMErrorData-${rootname}.$plane.png rawCorrBPMErrorData-${rootname}.$plane.eps}
        
        exec sddsplot3 rawCorrBPMErrorData-${rootname}.$plane $rootname.sdds.env  -graph=line,vary -layout=1,2 -mode=y=offset \
          -limits=xMinimum=0,xMaximum=${timeLimit} \
          -leg=editCommand=%/Mean\[U://%/:OFB//%/Long//%/HistAM\]//%/:OFB:LongErrorHistAM\]//%/X:Error// \
          -col=Time,U*S27A:${plane}3*Error* -leg=spec=S27A:H3:Error\
          -col=Time,U*S27A:${plane}3*Drive* -leg=spec=S27A:H3:Drive\
          -col=Time,U*S27B:${plane}4*Error* -leg=spec=S27B:H4:Error \
          -col=Time,U*S28A:${plane}3*Error* -leg=spec=S28A:H3:Error \
          -col=Time,U*S28B:${plane}4*Error* -leg=spec=S28B:H4:Error \
          "-yLabel=Current Change (Amps)" \
          "-topline=Closed Loop Single Horizontal Corrector S27A:H3 with AFG Waveform Applied" -end \
          -col=Time,U*S*P0*${coord}* \
          -col=Time,U*S*P2*${coord}* \
          -col=Time,U*S*P3*${coord}*  \
          -col=Time,U*S*P4*${coord}*  \
          "-yLabel=BPM Error (mm)" \
          "-title=S27A:${plane}3 Drive Corrector" &

    }
    SetStatus "done."
    return
}

proc PlotFFTData {args} {
    global outputDir
    
    cd $outputDir
    
    set dirs [concat [glob -nocomplain -type d *Kp*Ki?] [glob -nocomplain -type d *Kp*Ki??] [glob -nocomplain -type d *Kp*Ki???]]
    

    catch {exec rm HVDriveVectorFFTs.png HVDriveVectorFFTs.eps}
    set files ""
    set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4 S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    puts $dirs
    
    foreach plane {H V} coord {X Y} {
        set corrList [list S27A:${plane}3 S27B:${plane}4 S28A:${plane}3 S28B:${plane}4]
        foreach dir $dirs {
            foreach corr $corrList {
                
            }
        }
    }
    

    return
   
}

proc ProcessData1 {args} {
    global rootname
    ProcessData -rootname $rootname
}

#proc ProcessCouplingData {args} {
#    foreach plane {H V} {
#        ProcessData -rootname $rootname -plane $plane -coupling 1
#    }
#}

proc ProcessData {args} {
    set rootname ""
    APSParseArguments {rootname}
    
    global outputDir OAGGlobal
    
    cd $outputDir
    
   # catch {exec mkdir -p $dir/$rootname}
    
    set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4  S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    set AFGList {AFG1 AFG2 AFG3 AFG4 AFG5 AFG6 AFG7 AFG8}
    
    if ![file exist ${rootname}-perturbWaveform.sdds] {
        return -code error "Corrector drive waveform file does not exist!"
    }
    if ![file exist correctorIndex.sdds] {
        exec sddsmakedataset correctorIndex.sdds -col=CorrectorName,type=string -data=S27A:H3,S27B:H4,S28A:H3,S28B:H4,S27A:V3,S27B:V4,S28A:V3,S28B:V4 \
          -col=Index,type=long -data=0,1,2,3,4,5,6,7
    }
    set corrWaveformFile  ${rootname}-perturbWaveform.sdds
    foreach corr $corrList AFG $AFGList {
        if [regexp {H} $corr] {
            set plane H
            set otherPlane V
        } elseif [regexp {V} $corr] {
            set plane V
            set otherPlane H
        } else {
            return -code error "Invalid corrector - $corr given"
        }
        SetStatus "process data for $rootname $corr..."
        set freq_Index [exec sddsprocess $corrWaveformFile -pipe=out -match=par,WaveformPV=*${AFG}* \
                          |  sddsfft -pipe \
                          -col=Index,Waveform \
                          | sddsprocess -pipe \
                          -redefine=col,Freq_Index,i_row \
                          -process=FFTWaveform,maximum,FFT_Waveform_Frequency,functionOf=Freq_Index,position \
                          | sdds2stream -pipe=in -param=FFT_Waveform_Frequency]
        SetStatus "$corr freq_index=$freq_Index"
        if [catch {exec sddsfft ${rootname}.sdds \
                     -fullOutput \
                     -col=Index,U*P*LongErrorHistAM,U*${plane}*OFB*,U*${otherPlane}*OFB*  -pipe=out \
                     | sddsprocess -pipe=in $rootname/${rootname}-${corr}.sdds.fft \
                     -redefine=col,Freq_Index,i_row } result] {
            return -code error "Error in fft1: $result"
        }
        
        # Filter the original file to only pick out a single FFT point for each quantity
        foreach coord {X Y} {
            if [catch {exec sddsprocess -pipe=out $rootname/${rootname}-$corr.sdds.fft \
                         -filter=col,Freq_Index,[expr ${freq_Index} - 0.5],[expr ${freq_Index} + 0.5] \
                         | sddsconvert -pipe \
                         -edit=col,FFTU*Error*,%/FFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT/ \
                         -edit=col,FFTU*Drive*,%/FFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT/ \
                         -edit=col,RealFFTU*Error*,%/RealFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Real/ \
                         -edit=col,ImagFFTU*Error*,%/ImagFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Imag/ \
                         -edit=col,ArgFFTU*Error*,%/ArgFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Arg/ \
                         -edit=col,RealFFTU*Drive*,%/RealFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Real/ \
                         -edit=col,ImagFFTU*Drive*,%/ImagFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Imag/ \
                         -edit=col,ArgFFTU*Drive*,%/ArgFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Arg/ \
                         -delete=col,*${plane}2*,*${otherPlane}2* \
                         | sddsenvelope -pipe \
                         -mean=f,Freq_Index,S* \
                         | tee $rootname/${rootname}-${corr}.sdds.env \
                         | sddscollect -pipe=in  $rootname/${rootname}-${corr}.sdds.collect.$coord \
                         -col=suffix=_FFTMean,col=FFT -col=suffix=_FFT_RealMean,col=FFT_Real \
                         -col=suffix=_FFT_ImagMean,col=FFT_Imag -col=suffix=_FFT_ArgMean,col=FFT_Arg } result] {
                return -code error "Error processing data1: $result"
            }
        
            if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord \
                         -pipe=out  -match=col,Rootname=*_${coord}_Error \
                         -edit=col,BPMName,Rootname,%/_${coord}_Error// \
                         |  sddsxref -pipe $OAGGlobal(SRLatticesDirectory)/default/aps.twi  \
                         -nowarnings -takee=s -match=BPMName=ElementName \
                         | sddssort -pipe -col=s \
                         | sddsconvert -pipe=in -retain=col,BPMName,FFT*  $rootname/${rootname}-${corr}.sdds_BPMErrorVector.sdds.$coord } result] {
                return -code error "Error in processing data2: $result"
                
            }
        }
        # Create separate files for bpm error mean fft quantities at the driven frequency
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord -pipe=out \
                     -match=col,Rootname=*${plane}*_Error \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Error// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorErrorVector.sdds } result] {
                return -code error "Error in processing data3: $result"
        }
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord -pipe=out \
                     -match=col,Rootname=*${plane}*_Drive \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Drive// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorDriveVector.sdds } result] {
            return -code error "Error in processing data4: $result"
        }
        #other plane
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord -pipe=out \
                     -match=col,Rootname=*\[HV\]*_Error \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Error// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index   \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorErrorVector.sdds.all  } result] {
                return -code error "Error in processing data3a: $result"
        }
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord -pipe=out \
                     -match=col,Rootname=*\[HV\]*_Drive \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Drive// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index  \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorDriveVector.sdds.all } result] {
            return -code error "Error in processing data4a: $result"
        }
    }
    SetStatus "done."
}

proc ComputeResponseMatrix {args} {
    set plot 0
    APSParseArguments {plot}
    
    global outputDir rootname
    
    cd $outputDir
    set process 0
    set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4  S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    set AFGList {AFG1 AFG2 AFG3 AFG4 AFG5 AFG6 AFG7 AFG8}
    foreach corr $corrList {
        foreach suffix {.sdds_BPMErrorVector.sdds.X} {
            if ![file exist $rootname/${rootname}-${corr}$suffix] {
                set process 1
                break
            }
        }
    }
    if $process {
        if [catch {ProcessData -rootname $rootname} result] {
            return -code error "Error processing data: $result"
        }
    }
    
    set tmpRoot /tmp/[APSTmpString]
   
    foreach corr $corrList AFG $AFGList {
        if [regexp {H} $corr] {
            set plane H
            set coord X
            set otherCoord Y
            set pl h
        } else {
            set plane V
            set coord Y
            set otherCoord X
            set pl v
        }
        SetStatus "working on $rootname $corr ..."
        set driveAmplitude [exec sddsprocess ${rootname}-perturbWaveform.sdds -pipe=out \
                              -match=par,WaveformPV=*${AFG}*  \
                              | sddsfft -pipe \
                              -col=Index,Waveform \
                              | sddsprocess -pipe \
                              -process=FFTWaveform,maximum,FFT_Waveform_Amplitude \
                              | sdds2stream -pipe=in -param=FFT_Waveform_Amplitude]
        set nameList {BPMError CouplingBPMError Drive Error DriveAll ErrorAll} 
        set suffixList [list .sdds_BPMErrorVector.sdds.$coord .sdds_BPMErrorVector.sdds.$otherCoord  .sdds_CorrectorDriveVector.sdds .sdds_CorrectorErrorVector.sdds .sdds_CorrectorDriveVector.sdds.all .sdds_CorrectorErrorVector.sdds.all]
        foreach name $nameList suffix $suffixList {
            if [catch {exec sddsprocess $rootname/${rootname}-${corr}$suffix -pipe=out \
                         "-redefine=col,${corr}_${name},FFT_Arg 0.0 < ? FFT -1.0 * : FFT 1.0 * \$" \
                         | sddsconvert -pipe=in $tmpRoot.$corr.$name \
                         -delete=col,FFT* } result] {
                return -code error "Error in computing matrix1: $result"
            }
            lappend ${plane}${name}FileList $tmpRoot.$corr.$name
        }
        
        if [catch {exec sddsprocess $tmpRoot.$corr.Drive $tmpRoot.$corr.q_mat \
                     "-redefine=col,${corr}_Drive,CorrectorName \"${corr}\" strmatch ?  ${driveAmplitude} : 0.0 \$" } result] {
            return -code error "Error in compute matrix2: $result"
        }
        lappend ${plane}qVectorFileList $tmpRoot.${corr}.q_mat
        lappend ${plane}QfromCeVectorFileList $tmpRoot.${corr}.Error
        lappend ${plane}QVectorFileList $tmpRoot.${corr}.Drive
        lappend AllDriveList $tmpRoot.$corr.DriveAll
        lappend AllErrorList $tmpRoot.$corr.ErrorAll
    }
    foreach plane {H V} {
        set modelMatrix [file readlink $rootname/${plane}rm]
        set ${plane}ModelMatrix $modelMatrix
        if [regexp {oagData} $modelMatrix] {
            set ${plane}ModelIRM [file dirname $modelMatrix]/irm 
        } else {
            set fileroot [file tail $modelMatrix]
            set ${plane}ModelIRM [file dirname $modelMatrix]/irm_$fileroot
        }
    }
    
    foreach plane {H V}  otherCoord {x y} {
        if [catch {eval exec sddsxref [set ${plane}BPMErrorFileList] $rootname/${rootname}_BPMErrorMatrix_$plane.sdds -take=S*
            eval exec sddsxref [set ${plane}CouplingBPMErrorFileList] $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling -take=S*
            eval exec sddsxref [set ${plane}ErrorFileList] $rootname/${rootname}_CorrectorErrorMatrix_$plane.sdds -take=S* -nowarnings
            eval exec sddsxref [set ${plane}DriveFileList] $rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds -take=S* -nowarnings 
            eval exec sddsxref [set ${plane}qVectorFileList] $rootname/${rootname}_qMatrix_$plane.sdds -take=S* -nowarnings
            eval exec sddsxref [set ${plane}QVectorFileList] $rootname/${rootname}_QMatrix_$plane.sdds -take=S* -nowarnings 
            eval exec sddsxref [set ${plane}QfromCeVectorFileList] $rootname/${rootname}_QfromCeMatrix_$plane.sdds -take=S* -nowarnings 
        } result] {
            return -code error "Error in compute matrix3: $result"
        }
        if [catch {exec sddsmatrixop  $rootname/${rootname}_QfromCeMatrix_$plane.sdds  $rootname/${rootname}_QfromCeMinusCdMatrix_$plane.sdds \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds -add } result] {
            return -code error "Error in compute matrix4: $result"
        }
        set modelMatrix [set ${plane}ModelMatrix]
        set modelIRM [set ${plane}ModelIRM]
        # Now, compute the R matrix -
        
        if [catch {exec sddsmatrixop -pipe=out $modelMatrix -push=$rootname/${rootname}_CorrectorErrorMatrix_$plane.sdds -multiply  \
                     -push=$rootname/${rootname}_QMatrix_$plane.sdds -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds \
                     -take=BPMName \
                     | sddsconvert -pipe=in $rootname/R.mat.$plane \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R.mat.$plane: $result"
        }
        if [catch {exec sddsmatrixop $modelIRM $rootname/I.mat.$plane -push=$rootname/R.mat.$plane -multiply } result] {
            return -code error "Error creating I.mat: $result"
        }
        if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix_$plane.sdds \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds \
                     -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds \
                     -take=BPMName \
                     | sddsconvert -pipe=in $rootname/R_bpm.mat.$plane \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R_bpm.mat: $result"
        }
        
        if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds \
                     -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling \
                     -take=BPMName \
                     | sddsconvert -pipe=in $rootname/R_bpm_coupling.mat.$plane \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R_bpm.mat: $result"
        }
        
        if [catch {exec sddsmatrixop $modelIRM $rootname/I_bpm.mat.$plane -push=$rootname/R_bpm.mat.$plane -multiply } result] {
            return -code error "Error creating I_bpm.mat.$plane: $result"
        }
        if [catch {exec  sddspseudoinverse $rootname/R.mat.$plane $rootname/irm_R.mat.$plane \
                     -oldColumnNames=ControlName \
                     -newColumnNames=BPMName \
                     -minimum=0.0 \
                     -uMatrix=$rootname/R.mat.u.$plane \
                     -sFile=$rootname/R.mat.s.$plane \
                     -vMatrix=$rootname/R.mat.v.$plane } result] {
            return -code error "Error creating irm_R.mat.$plane : $result"
        }
        
        if [catch {exec sddspseudoinverse $modelMatrix $rootname/irm.$plane \
                     -oldColumnNames=ControlName \
                     -newColumnNames=BPMName \
                     -minimum=0.0 \
                     -uMatrix=$rootname/rm.u.$plane \
                     -sFile=$rootname/rm.s.$plane \
                     -vMatrix=$rootname/rm.v.$plane } result] {
            return -code error "Error create model ${plane}irm: $result"
        }
        
        if [catch {exec sddspseudoinverse $rootname/R_bpm.mat.$plane $rootname/irm_R_bpm.mat.$plane \
                     -oldColumnNames=ControlName \
                     -newColumnNames=BPMName \
                     -minimum=0.0 \
                     -uMatrix=$rootname//R_bpm.mat.u.$plane \
                 -sFile=$rootname/R_bpm.mat.s.$plane \
                     -vMatrix=$rootname/R_bpm.mat.v.$plane } result] {
            return -code error "Error create irm_R_bpm.mat.$plane: $result"
        }
    }
    #compute the whole matrix 32bpms x 8 correctors
    #first make 32bpm x 8 corrector model matrix -- non dialog -zero
    set tmpRoot /tmp/[APSTmpString]
    if [catch {eval exec sddsxref $AllDriveList $rootname/${rootname}_CorrectorDriveMatrix.sdds -take=S* -nowarnings
        eval exec sddsxref $AllErrorList $rootname/${rootname}_CorrectorErrorMatrix.sdds -take=S* -nowarnings } result] {
        return -code error "Error creating all corrector matrix: $result"
    }
    if [catch {exec sddsprocess $rootname/Hrm $tmpRoot.h \
                 -reedit=col,BPMName,ei/:X/ \
                 -define=col,S27A:V3,0,units=mm/A \
                 -define=col,S27B:V4,0,units=mm/A \
                 -define=col,S28A:V3,0,units=mm/A \
                 -define=col,S28B:V4,0,units=mm/A } result] {
        return -code error "Error creating x bpm matrix: $result"
    }
    if [catch {exec sddsprocess $rootname/Vrm -pipe=out \
                 -reedit=col,BPMName,ei/:Y/ \
                 -define=col,S27A:H3,0,units=mm/A \
                 -define=col,S27B:H4,0,units=mm/A \
                 -define=col,S28A:H3,0,units=mm/A \
                 -define=col,S28B:H4,0,units=mm/A \
                 | sddssortcolumn -pipe=in $tmpRoot.v \
                 -sortList=BPMName,[join $corrList ,]} result] {
        return -code error "Error creating y bpm matrix: $result"
    }
    if [catch {exec sddscombine $tmpRoot.h $tmpRoot.v -merge $rootname/rm.all -overwrite } result] {
        return -code error "Error creating ALL rm matrix: $result"
    }
    #create bpm error full matrix
    APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.h $tmpRoot.v"
    if [catch {exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_H.sdds -edit=col,S*,%/_BPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.h1 -reedit=col,BPMName,ei/:X/
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_H.sdds.coupling -edit=col,S*,%/_CouplingBPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.h2 -reedit=col,BPMName,ei/:Y/
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_V.sdds -edit=col,S*,%/_BPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.v2 -reedit=col,BPMName,ei/:Y/
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_V.sdds.coupling -edit=col,S*,%/_CouplingBPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.v1 -reedit=col,BPMName,ei/:X/} result] {
        return -code error "Error creating full bpm error matrix1: $result"
    }
    APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.h1 $tmpRoot.h2 $tmpRoot.v1 $tmpRoot.v2"
    
    if [catch {exec sddscombine $tmpRoot.h1 $tmpRoot.h2 -merge $tmpRoot.hbpm -over
        exec sddscombine $tmpRoot.v1 $tmpRoot.v2 -merge $tmpRoot.vbpm -over
        exec sddsxref $tmpRoot.hbpm $tmpRoot.vbpm -take=S* $rootname/${rootname}_BPMErrorMatrix.sdds } result] {
        return -code error "Error crewating full bpm error matrix2: $result"
    }
     APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.hbpm $tmpRoot.vbpm"
    #create full corrector error matrix
    
    #compute R_mat = modelMatrix * corrError * inverse of CorrDrive * -1
    if [catch {exec sddsmatrixop -pipe=out $rootname/rm.all -push=$rootname/${rootname}_CorrectorErrorMatrix.sdds -multiply \
                 -push=$rootname/${rootname}_CorrectorDriveMatrix.sdds -invert -multiply -scalarMultiply=-1.0 \
                 | sddsxref -pipe $rootname/rm.all -take=BPMName  \
                 | sddsconvert -pipe=in $rootname/R.mat  \
                 -rename=col,doubleColumn0=S27A:H3 \
                 -rename=col,doubleColumn1=S27B:H4 \
                 -rename=col,doubleColumn2=S28A:H3 \
                 -rename=col,doubleColumn3=S28B:H4 \
                 -rename=col,doubleColumn4=S27A:V3 \
                 -rename=col,doubleColumn5=S27B:V4 \
                 -rename=col,doubleColumn6=S28A:V3 \
                 -rename=col,doubleColumn7=S28B:V4 } result] {
        return -code error "Error creating full R.mat matrix: $result"
    }
    
    if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix.sdds \
                 -push=$rootname/${rootname}_CorrectorDriveMatrix.sdds \
                 -invert -multiply -scalarMultiply=-1.0 \
                 | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix.sdds \
                 -take=BPMName \
                 | sddsconvert -pipe=in $rootname/R_bpm.mat \
                 -rename=col,doubleColumn0=S27A:H3 \
                 -rename=col,doubleColumn1=S27B:H4 \
                 -rename=col,doubleColumn2=S28A:H3 \
                 -rename=col,doubleColumn3=S28B:H4 \
                 -rename=col,doubleColumn4=S27A:V3 \
                 -rename=col,doubleColumn5=S27B:V4 \
                 -rename=col,doubleColumn6=S28A:V3 \
                 -rename=col,doubleColumn7=S28B:V4} result] {
        return -code error "Error creating full R_bpm.mat: $result"
    }
    #plot
    
    if {$plot} {
        PlotMatrix
    }
    SetStatus "done."
   
}

proc PlotMatrix {args} {
    set plane ""
    APSParseArguments {plane}
    
    global outputDir rootname
    cd $outputDir
    if [string length $plane] {
        set planeList $plane
    } else {
        set planeList {H V}
    }
    set currentDir [pwd]
    foreach plane $planeList {
        cd $currentDir
        if  {![file exist $outputDir/$rootname/R.mat.$plane]} {
            SetStatus "$plane Matrix file  $rootname/R.mat.$plane  are not computed for $rootname."
            return
        }
        cd $rootname
        
        exec sddsplot -mode=y=log,y=spec -graph=symb,sca=2,conn,vary,vary=sub \
          "-topline=$plane plane Singular Values for Data Set ${rootname}" \
          "-title=Comparison Between model (loaded) and Corrected Response Matrices" \
          -col=Index,SingularValues -leg=spec=R_Corr R.mat.s.$plane \
          -col=Index,SingularValues -leg=spec=R_BPM R_bpm.mat.s.$plane \
          -col=Index,SingularValues -leg=spec=FBC rm.s.$plane  &
        
        exec sddsplot -graph=symb,sca=2,conn,vary,vary=sub \
          -sep=nameIndex -groupby=nameIndex \
          "-topline=$plane plane  U Matrix Vectors for Data Set ${rootname}" \
          "-title=Comparison Between Model (loaded) and Corrected Response Matrices" \
          -col=BPMName,SV* -leg=spec=R_Corr R.mat.u.$plane \
          -col=BPMName,SV* -leg=spec=R_BPM R_bpm.mat.u.$plane \
          -col=BPMName,SV* -leg=spec=FBC rm.u.$plane  &
        
        exec sddsplot -graph=symb,sca=2,conn,vary,vary=sub \
          -sep=nameIndex -groupby=nameIndex \
          "-topline=$plane plane V Matrix Vectors for Data Set ${rootname}" \
          "-title=Comparison Between Model (loaded) and Corrected Response Matrices" \
          -col=ControlName,SV* -leg=spec=R_Corr R.mat.v.$plane \
          -col=ControlName,SV* -leg=spec=R_BPM R_bpm.mat.v.$plane \
          -col=ControlName,SV* -leg=spec=FBC rm.v.$plane &
        
        exec sddsplot -sep=nameIndex -groupBy=nameIndex -layout=2,2 \
          "-topline=$plane plane Comparison between Model (loaded) and Measured Response Matrix" \
          -col=BPMName,S27A:${plane}3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat.$plane \
          -col=BPMName,S27A:${plane}3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat.$plane \
          -col=BPMName,S27A:${plane}3 -graph=symb,sca=2,conn,type=2,subtype=2 -leg=spec=FBC ${plane}rm \
          "-yLabel=S27A${plane}3 Response (mm/A)" \
          "-title=Dataset ${rootname}" -end \
          -col=BPMName,S27B:${plane}4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat.$plane \
          -col=BPMName,S27B:${plane}4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat.$plane \
          -col=BPMName,S27B:${plane}4 -graph=symb,sca=2,conn,type=2,subtype=2 -leg=spec=FBC ${plane}rm \
          "-yLabel=S27B${plane}4 Response (mm/A)" \
          "-title=Dataset ${rootname}" -end \
          -col=BPMName,S28A:${plane}3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat.$plane \
          -col=BPMName,S28A:${plane}3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat.$plane \
          -col=BPMName,S28A:${plane}3 -graph=symb,sca=2,conn,type=2,subtype=2 -leg=spec=FBC ${plane}rm \
          "-yLabel=S28A${plane}3 Response (mm/A)" \
          "-title=Dataset ${rootname}" -end \
          -col=BPMName,S28B:${plane}4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat.$plane \
          -col=BPMName,S28B:${plane}4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat.$plane \
          -col=BPMName,S28B:${plane}4 -graph=symb,sca=2,conn,type=2,subtype=2 -leg=spec=FBC ${plane}rm \
          "-yLabel=S28B${plane}4 Response (mm/A)" \
          "-title=Dataset ${rootname}" \&
    }
    #ploting coupling
     exec sddsplot -sep=nameIndex -match=col,BPMName=*Y -groupBy=nameIndex -layout=2,2 \
      "-topline=H plane Coupling matrix Comparison between Model  and Measured" \
      -col=BPMName,S27A:H3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S27A:H3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S27A:H3 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end \
      -col=BPMName,S27B:H4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S27B:H4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S27B:H4 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end \
      -col=BPMName,S28A:H3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S28A:H3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S28A:H3 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end \
      -col=BPMName,S28B:H4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S28B:H4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S28B:H4 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end &
     exec sddsplot -sep=nameIndex -match=col,BPMName=*X -groupBy=nameIndex -layout=2,2 \
      "-topline=V plane Coupling matrix Comparison between Model  and Measured" \
       -col=BPMName,S27A:V3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S27A:V3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S27A:V3 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end \
      -col=BPMName,S27B:V4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S27B:V4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S27B:V4 Response (mm/A)" \
      "-title=Dataset ${rootname}"  -end \
      -col=BPMName,S28A:V3 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S28A:V3 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S28A:V3 Response (mm/A)" \
      "-title=Dataset ${rootname}"   -end \
      -col=BPMName,S28B:V4 -graph=symb,sca=2,conn,type=0,subtype=0 -leg=spec=R_Corr R.mat \
      -col=BPMName,S28B:V4 -graph=symb,sca=2,conn,type=1,subtype=1 -leg=spec=R_BPM R_bpm.mat \
      "-yLabel=S28B:V4 Response (mm/A)" \
      "-title=Dataset ${rootname}" -end &
    

}

proc SetOutputDataDir {args} {
    global outputDir plane
    set outputDir [APSGoToDailyDirectory -subdirectory S27IntegratedStudiesAll]
    return
    switch $plane {
        H {
            set outputDir [APSGoToDailyDirectory -subdirectory S27IntegratedStudies]/Horizontal
        }
        V {
            set outputDir [APSGoToDailyDirectory -subdirectory S27IntegratedStudies]/Vertical
        }
    }
    if ![file exist $outputDir] {
        exec mkdir -p $outputDir
    }
}

proc ChangeModelMatrix {args} {
    set type ""
    APSParseArguments {type}
    global modelDir outputDir HModelDir VModelDir rootname
    if ![string length $type] {
        set type prevmeas
    }
    switch $type {
        elegant {
            set modelDir /home/helios/oagData/sr/rtfeedback/lattices/default
            set HModelDir h.2018-0523.00
            set VModelDir v.2018-0523.00
        }
        prevmeas {
            set modelDir $outputDir
            set num [lindex [split $rootname "-"] 1]
            set root [lindex [split $rootname "-"] 0]
            if {[string length $num] && $num>0} {
                set num0 [expr $num -1]
                if {$num0>0} {
                    set HModelDir ${root}-$num0
                    set VModelDir ${root}-$num0 
                } else {
                    set HModelDir ${root}
                    set VModelDir ${root}
                }
            } else {
                #use elegant matrix still
                set modelDir /home/helios/oagData/sr/rtfeedback/lattices/default
                set HModelDir h.2018-0523.00
                set VModelDir v.2018-0523.00
            }
        }
    }
}

proc GenerateAFGDriveWaveform {args} {
    set load 0
    set plot 1
    APSParseArguments {load plot}
    
    global CorrH1Np CorrH1Freq CorrH2Np CorrH2Freq CorrH3Np CorrH3Freq CorrH4Np CorrH4Freq HCurrentAmp
    global CorrV1Np CorrV1Freq CorrV2Np CorrV2Freq CorrV3Np CorrV3Freq CorrV4Np CorrV4Freq VCurrentAmp
    
    set sampleRate [exec cavget -list=U:S27:OFB:DSP:LoopRateM ]
  #  set sampleRate 22600.0
    global rootname outputDir
    
    set N 20480
    set PI 3.141592653589793238462643
    set time [expr $N * 1.0 / $sampleRate]
    set tmpRoot /tmp/[APSTmpString]
    set fileList ""
    foreach plane {H V} {
        foreach i {1 2 3 4} {
            set Np [set Corr${plane}${i}Np]
            set freq [format %.3f [expr $Np * 1.0 /($N -1.0) * $sampleRate]]
            set Corr${plane}${i}Freq $freq
            if {$plane=="V"} {
                set AFG AFG[expr $i+4]
            } else {
                set AFG AFG$i
            }
            set CurrentAmp [set ${plane}CurrentAmp]
            
            set Corr${plane}${i}Freq $freq
            if [catch {exec sddssequence -pipe=out -define=Index  -sequence=begin=0,end=[expr $N-1],interval=1,delta=1 \
                         | sddsprocess -pipe=in  $tmpRoot.$plane.$i  \
                         "-reprint=par,Description,Perturbation Waveform at $freq Hz" \
                         "-reprint=par,NpString,Np=$Np" \
                         "-redefine=par,Np,$Np,type=long" \
                         "-redefine=col,time,Index $sampleRate /,units=s" \
                         "-reprint=par,WaveformPV,U:S27:OFB:$AFG:WaveformC" \
                         "-redefine=col,Waveform,2.0 $PI * Index * $Np * $N / sin $CurrentAmp *,units=Amps" } result] {
                return -code error "GenerateAFGDriveWaveform00: $result"
            }
            lappend fileList $tmpRoot.$plane.$i
        }
    }
    if [catch {eval exec sddscombine $fileList $outputDir/${rootname}-perturbWaveform.sdds -overwrite} result] {
        return -code error "GenerateAFGDriveWaveform1: $result"
    }
    set freq [format %.0f $freq]
    if $plot {
        exec  sddsplot -grap=line,vary -scale=-0.002,0.1,0,0 -split=page -col=time,Waveform -leg=par=NpString $outputDir/${rootname}-perturbWaveform.sdds  \
          "-topline=Perturbation Waveform around $freq Hz." &
        exec  sddsplot -grap=line,vary -scale=0.7,0.91,0,0 -split=page -col=time,Waveform  -leg=par=NpString $outputDir/${rootname}-perturbWaveform.sdds   \
          "-topline=Perturbation Waveform around $freq Hz."  &
    }
    if [catch {exec sddsfft $outputDir/${rootname}-perturbWaveform.sdds -pipe=out -col=Index,Waveform -full \
                 | sddsprocess -pipe  \
                 "-reprint=par,Description,Perturbation FFT Waveform at $freq Hz." \
                 "-process=FFTWaveform,max,FFTWaveformMax" \
                 "-process=FFTWaveform,max,FFTPeakFreq,functionof=f,position" \
                 | sddsprocess -pipe=in  $outputDir/${rootname}-perturbWaveform.fft "-redefine=col,ImagFFTWaveformAbs,ImagFFTWaveform abs" \
                 "-process=ImagFFTWaveformAbs,max,ImagFFTWaveformAbsMax" \
                 "-process=ImagFFTWaveformAbs,max,ImagAbsPeakFreq,function=f,position" \
                 "-process=ImagFFTWaveformAbs,max,ImagPeak,functionof=ImagFFTWaveform,position" \
             } result] {
        return -code error "computefrequency1: $result"
    }
    if ${plot} {
        exec sddsplot $outputDir/${rootname}-perturbWaveform.fft -scale=1e-3,3e-3,0,0 -split=page -grap=bar,vary,vary=sub  -col=f,FFT* \
          "-topline=Perturbation FFT Waveform around $freq Hz."  \
          -par=FFTPeakFreq,FFTWaveformMax -leg=par=NpString -grap=sym,var,vary=sub,scale=2 &
    }
    if $load {
        SetStatus "loading AFG..."
        if [catch {exec cavput -list=U:S27:OFB:AFG -range=begin=1,end=8,interval=1 -list=:LastValidElementC=20480 } result] {
            return -code error "Error setting the AFG waveform length to 20480: $result"
        }
        foreach file $fileList {
            if [catch {exec sddswput $file -pend=30} result] {
                return -code error "Error loading $file to AFG: $result"
            }
            after 1000
        }
        if [catch {exec cavput -list=U:S27:OFB:AFG -list=1,2,3,4,5,6,7,8 -list=:EnableC=2 -pend=20 } result] {
            return -code error "Error setting enable AFG waveform : $result"
        }
        #1,2,3,4 for H plane, 5,6,7,8 for V plane
        if [catch {exec cavput -list=U:S27:OFB:AFG -list=1,2,3,4 -list=:PlaneC=0 -pend=20} result] {
            return -code error "Error setting AFG 1,2,3,4 to H plane: $result"
        }
        if [catch {exec cavput -list=U:S27:OFB:AFG -list=5,6,7,8 -list=:PlaneC=1 -pend=20} result] {
            return -code error "Error setting AFG 5,6,7,8 to V plane: $result"
        }
        #AFG1 for S27A3, AFG2 for S27B4, 5 for S28A3, 6 for S28B4
        #AFG5 for S27A:V3 ...
        if [catch {exec cavput \
                     -list=U:S27:OFB:AFG1:SignalIndex2C=1,U:S27:OFB:AFG2:SignalIndex2C=2,U:S27:OFB:AFG3:SignalIndex2C=5,U:S27:OFB:AFG4:SignalIndex2C=6 \
                     -pend=30} result] {
            return -code error "Error connecting H corrector to AFG: $result"
        }
        if [catch {exec cavput \
                     -list=U:S27:OFB:AFG5:SignalIndex2C=1,U:S27:OFB:AFG6:SignalIndex2C=2,U:S27:OFB:AFG7:SignalIndex2C=5,U:S27:OFB:AFG8:SignalIndex2C=6 \
                     -pend=30} result] {
            return -code error "Error connecting V corrector to AFG: $result"
        }
        SetStatus "done."
    }
    APSAddToTmpFileList -ID daq -fileList $fileList
}

proc LoadAFG {args} {
    set generate 0
    set plot 1
    APSParseArguments {generate plot}
    global outputDir rootname
    
    if [catch {GenerateAFGDriveWaveform -plot $plot -load 1 } result] {
        return -code error "Error generating AFG drive waveform: $result"
    }
   
}

proc ProcessDataNoAverage {args} {
    global outputDir rootname
    cd $outputDir

    
    set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4  S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    set AFGList {AFG1 AFG2 AFG3 AFG4 AFG5 AFG6 AFG7 AFG8}
    
    if ![file exist ${rootname}-perturbWaveform.sdds] {
        return -code error "Corrector drive waveform file does not exist!"
    }

    if ![file exist correctorIndex.sdds] {
        exec sddsmakedataset correctorIndex.sdds -col=CorrectorName,type=string -data=S27A:H3,S27B:H4,S28A:H3,S28B:H4,S27A:V3,S27B:V4,S28A:V3,S28B:V4 \
          -col=Index,type=long -data=0,1,2,3,4,5,6,7
    }
    set corrWaveformFile  ${rootname}-perturbWaveform.sdds
    foreach corr $corrList AFG $AFGList {
        if [regexp {H} $corr] {
            set plane H
            set otherPlane V
        } elseif [regexp {V} $corr] {
            set plane V
            set otherPlane H
        } else {
            return -code error "Invalid corrector - $corr given"
        }
        SetStatus "process data for $rootname $corr..."
        set freq_Index [exec sddsprocess $corrWaveformFile -pipe=out -match=par,WaveformPV=*${AFG}* \
                          |  sddsfft -pipe \
                          -col=Index,Waveform \
                          | sddsprocess -pipe \
                          -redefine=col,Freq_Index,i_row \
                          -process=FFTWaveform,maximum,FFT_Waveform_Frequency,functionOf=Freq_Index,position \
                          | sdds2stream -pipe=in -param=FFT_Waveform_Frequency]
        SetStatus "$corr freq_index=$freq_Index"
        if [catch {exec sddsfft ${rootname}.sdds \
                     -fullOutput \
                     -col=Index,U*P*LongErrorHistAM,U*${plane}*OFB*,U*${otherPlane}*OFB*  -pipe=out \
                     | sddsprocess -pipe=in $rootname/${rootname}-${corr}.sdds.fft \
                     -redefine=col,Freq_Index,i_row } result] {
            return -code error "Error in fft1: $result"
        }
        
        # Filter the original file to only pick out a single FFT point for each quantity
        foreach coord {X Y} {
            if [catch {exec sddsprocess -pipe=out $rootname/${rootname}-$corr.sdds.fft \
                         -filter=col,Freq_Index,[expr ${freq_Index} - 0.5],[expr ${freq_Index} + 0.5] \
                         | sddsconvert -pipe \
                         -edit=col,FFTU*Error*,%/FFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT/ \
                         -edit=col,FFTU*Drive*,%/FFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT/ \
                         -edit=col,RealFFTU*Error*,%/RealFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Real/ \
                         -edit=col,ImagFFTU*Error*,%/ImagFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Imag/ \
                         -edit=col,ArgFFTU*Error*,%/ArgFFTU://%/${coord}/_${coord}/%/:OFB:LongErrorHistAM/_Error_FFT_Arg/ \
                         -edit=col,RealFFTU*Drive*,%/RealFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Real/ \
                         -edit=col,ImagFFTU*Drive*,%/ImagFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Imag/ \
                         -edit=col,ArgFFTU*Drive*,%/ArgFFTU://%/${coord}/_${coord}/%/:OFB:LongDriveHistAM/_Drive_FFT_Arg/ \
                         -delete=col,*${plane}2*,*${otherPlane}2* \
                         | tee $rootname/${rootname}-${corr}.sdds.noenv \
                         | sddscollect -pipe=in  $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv \
                         -col=suffix=_FFT,col=FFT -col=suffix=_FFT_Real,col=FFT_Real \
                         -col=suffix=_FFT_Imag,col=FFT_Imag -col=suffix=_FFT_Arg,col=FFT_Arg } result] {
                return -code error "Error processing data1: $result"
            }
        
            if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv \
                         -pipe=out  -match=col,Rootname=*_${coord}_Error \
                         -edit=col,BPMName,Rootname,%/_${coord}_Error// \
                         |  sddsxref -pipe $OAGGlobal(SRLatticesDirectory)/default/aps.twi  \
                         -nowarnings -takee=s -match=BPMName=ElementName \
                         | sddssort -pipe -col=s \
                         | sddsconvert -pipe=in -retain=col,BPMName,FFT*  $rootname/${rootname}-${corr}.sdds_BPMErrorVector.sdds.$coord.noenv } result] {
                return -code error "Error in processing data2: $result"
                
            }
        }
        # Create separate files for bpm error mean fft quantities at the driven frequency
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv -pipe=out \
                     -match=col,Rootname=*${plane}*_Error \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Error// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index -reuse=page \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorErrorVector.sdds.noenv  } result] {
                return -code error "Error in processing data3: $result"
        }
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv -pipe=out \
                     -match=col,Rootname=*${plane}*_Drive \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Drive// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index -reuse=page  \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorDriveVector.sdds.noenv } result] {
            return -code error "Error in processing data4: $result"
        }
        #other plane
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv -pipe=out \
                     -match=col,Rootname=*\[HV\]*_Error \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Error// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index -reuse=page   \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorErrorVector.sdds.all.noenv  } result] {
                return -code error "Error in processing data3a: $result"
        }
        if [catch {exec sddsprocess $rootname/${rootname}-${corr}.sdds.collect.$coord.noenv -pipe=out \
                     -match=col,Rootname=*\[HV\]*_Drive \
                     | sddsprocess -pipe -reedit=col,Rootname,%/_Drive// \
                     | sddsconvert -pipe -rename=col,Rootname=CorrectorName \
                     | sddsxref -pipe correctorIndex.sdds -match=CorrectorName -take=Index -reuse=page \
                     | sddssort -pipe -col=Index \
                     | sddsconvert -pipe=in -retain=col,CorrectorName,FFT* \
                     $rootname/${rootname}-${corr}.sdds_CorrectorDriveVector.sdds.all.noenv } result] {
            return -code error "Error in processing data4a: $result"
        }
    }
    SetStatus "done."
    
}

proc ComputeResponseMatrixForEachDataSet {args} {
    global outputDir rootname
    cd $outputDir

    
     set process 0
    set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4  S27A:V3 S27B:V4 S28A:V3 S28B:V4}
    set AFGList {AFG1 AFG2 AFG3 AFG4 AFG5 AFG6 AFG7 AFG8}
    foreach corr $corrList {
        foreach suffix {.sdds_BPMErrorVector.sdds.X.noenv} {
            if ![file exist $rootname/${rootname}-${corr}$suffix] {
                set process 1
                break
            }
        }
    }
    if $process {
        if [catch {ProcessDataNoAverage} result] {
            return -code error "Error processing data: $result"
        }
    }
    set tmpRoot /tmp/[APSTmpString]
    
    foreach corr $corrList AFG $AFGList {
        if [regexp {H} $corr] {
            set plane H
            set coord X
            set otherCoord Y
            set pl h
        } else {
            set plane V
            set coord Y
            set otherCoord X
            set pl v
        }
        SetStatus "working on $rootname $corr ..."
        set driveAmplitude [exec sddsprocess ${rootname}-perturbWaveform.sdds -pipe=out \
                              -match=par,WaveformPV=*${AFG}*  \
                              | sddsfft -pipe \
                              -col=Index,Waveform \
                              | sddsprocess -pipe \
                              -process=FFTWaveform,maximum,FFT_Waveform_Amplitude \
                              | sdds2stream -pipe=in -param=FFT_Waveform_Amplitude]
        set nameList {BPMError CouplingBPMError Drive Error DriveAll ErrorAll} 
        set suffixList [list .sdds_BPMErrorVector.sdds.$coord .sdds_BPMErrorVector.sdds.$otherCoord  .sdds_CorrectorDriveVector.sdds .sdds_CorrectorErrorVector.sdds .sdds_CorrectorDriveVector.sdds.all .sdds_CorrectorErrorVector.sdds.all]
        foreach name $nameList suffix $suffixList {
            if [catch {exec sddsprocess $rootname/${rootname}-${corr}$suffix.noenv -pipe=out \
                         "-redefine=col,${corr}_${name},FFT_Arg 0.0 < ? FFT -1.0 * : FFT 1.0 * \$" \
                         | sddsconvert -pipe=in $tmpRoot.$corr.$name \
                         -delete=col,FFT* } result] {
                return -code error "Error in computing matrix1: $result"
            }
            lappend ${plane}${name}FileList $tmpRoot.$corr.$name
        }
        
        if [catch {exec sddsprocess $tmpRoot.$corr.Drive $tmpRoot.$corr.q_mat \
                     "-redefine=col,${corr}_Drive,CorrectorName \"${corr}\" strmatch ?  ${driveAmplitude} : 0.0 \$" } result] {
            return -code error "Error in compute matrix2: $result"
        }
        lappend ${plane}qVectorFileList $tmpRoot.${corr}.q_mat
        lappend ${plane}QfromCeVectorFileList $tmpRoot.${corr}.Error
        lappend ${plane}QVectorFileList $tmpRoot.${corr}.Drive
        lappend AllDriveList $tmpRoot.$corr.DriveAll
        lappend AllErrorList $tmpRoot.$corr.ErrorAll
    }
    
    foreach plane {H V} {
        set modelMatrix [file readlink $rootname/${plane}rm]
        set ${plane}ModelMatrix $modelMatrix
        if [regexp {oagData} $modelMatrix] {
            set ${plane}ModelIRM [file dirname $modelMatrix]/irm 
        } else {
            set fileroot [file tail $modelMatrix]
            set ${plane}ModelIRM [file dirname $modelMatrix]/irm_$fileroot
        }
    }
    
    foreach plane {H V}  otherCoord {x y} {
        if [catch {eval exec sddsxref [set ${plane}BPMErrorFileList] $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.noenv -take=S*
            eval exec sddsxref [set ${plane}CouplingBPMErrorFileList] $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling.noenv -take=S*
            eval exec sddsxref [set ${plane}ErrorFileList] $rootname/${rootname}_CorrectorErrorMatrix_$plane.sdds.noenv -take=S* -nowarnings
            eval exec sddsxref [set ${plane}DriveFileList] $rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds.noenv -take=S* -nowarnings 
            eval exec sddsxref [set ${plane}qVectorFileList] $rootname/${rootname}_qMatrix_$plane.sdds.noenv -take=S* -nowarnings
            eval exec sddsxref [set ${plane}QVectorFileList] $rootname/${rootname}_QMatrix_$plane.sdds.noenv -take=S* -nowarnings 
            eval exec sddsxref [set ${plane}QfromCeVectorFileList] $rootname/${rootname}_QfromCeMatrix_$plane.sdds.noenv -take=S* -nowarnings 
        } result] {
            return -code error "Error in compute matrix3: $result"
        }
        if [catch {exec sddsmatrixop  $rootname/${rootname}_QfromCeMatrix_$plane.sdds.noenv  $rootname/${rootname}_QfromCeMinusCdMatrix_$plane.sdds \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds.noenv -add } result] {
            return -code error "Error in compute matrix4: $result"
        }
        set modelMatrix [set ${plane}ModelMatrix]
        set modelIRM [set ${plane}ModelIRM]
        # Now, compute the R matrix -
        set npages [exec sdds2stream -npage=bar $rootname/${rootname}_CorrectorErrorMatrix_$plane.sdds.noenv]
        if ![file exist $rootname/${plane}rm.all] {
            set files [APSReplicateItem -item $modelMatrix -number $npages]
            if [catch {eval exec sddscombine $files $rootname/${plane}rm.all} result] {
                return -code error "Error creating $npages pages model matrix: $result"
            }
            if [catch {exec sddspseudoinverse $rootname/${plane}rm.all $rootname/${plane}irm.all \
                         -oldColumnNames=ControlName \
                         -newColumnNames=BPMName \
                         -minimum=0.0 \
                         -uMatrix=$rootname/${plane}rm.u.all \
                         -sFile=$rootname/${plane}rm.s.all \
                         -vMatrix=$rootname/${plane}rm.v.all } result] {
                return -code error "Error create model ${plane}irm: $result"
            }
        }
        if [catch {exec sddsmatrixop -pipe=out $rootname/${plane}rm.all -push=$rootname/${rootname}_CorrectorErrorMatrix_$plane.sdds.noenv -multiply  \
                     -push=$rootname/${rootname}_QMatrix_$plane.sdds.noenv -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.noenv \
                     -take=BPMName \
                     | sddsprocess -pipe -redefine=par,Page,i_page,type=long  \
                     | sddsprocess -pipe -reprint=par,PageString,Page%ld,Page \
                     | sddsconvert -pipe=in $rootname/R.mat.$plane.noenv \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R.mat.$plane: $result"
        }
        if [catch {exec sddsmatrixop $rootname/${plane}irm.all $rootname/I.mat.$plane.noenv -push=$rootname/R.mat.$plane.noenv -multiply } result] {
            return -code error "Error creating I.mat: $result"
        }
        if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.noenv \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds.noenv \
                     -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.noenv \
                     -take=BPMName \
                     | sddsprocess -pipe -redefine=par,Page,i_page,type=long  \
                     | sddsprocess -pipe -reprint=par,PageString,Page%ld,Page \
                     | sddsconvert -pipe=in $rootname/R_bpm.mat.$plane.noenv \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R_bpm.mat: $result"
        }
        
        if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling.noenv \
                     -push=$rootname/${rootname}_CorrectorDriveMatrix_$plane.sdds.noenv \
                     -invert -multiply -scalarMultiply=-1.0 \
                     | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix_$plane.sdds.coupling.noenv \
                     -take=BPMName \
                     | sddsprocess -pipe -redefine=par,Page,i_page,type=long  \
                     | sddsprocess -pipe -reprint=par,PageString,Page%ld,Page \
                     | sddsconvert -pipe=in $rootname/R_bpm_coupling.mat.$plane.noenv \
                     -rename=col,doubleColumn0=S27A:${plane}3 \
                     -rename=col,doubleColumn1=S27B:${plane}4 \
                     -rename=col,doubleColumn2=S28A:${plane}3 \
                     -rename=col,doubleColumn3=S28B:${plane}4 } result] {
            return -code error "Error creating R_bpm.mat: $result"
        }
        
        if [catch {exec sddsmatrixop $rootname/${plane}irm.all $rootname/I_bpm.mat.$plane.noenv -push=$rootname/R_bpm.mat.$plane.noenv -multiply } result] {
            return -code error "Error creating I_bpm.mat.$plane: $result"
        }
        if [catch {exec  sddspseudoinverse $rootname/R.mat.$plane.noenv $rootname/irm_R.mat.$plane.noenv \
                     -oldColumnNames=ControlName \
                     -newColumnNames=BPMName \
                     -minimum=0.0 \
                     -uMatrix=$rootname/R.mat.u.$plane.noenv \
                     -sFile=$rootname/R.mat.s.$plane.noenv \
                     -vMatrix=$rootname/R.mat.v.$plane.noenv } result] {
            return -code error "Error creating irm_R.mat.$plane : $result"
        }
       
        
        if [catch {exec sddspseudoinverse $rootname/R_bpm.mat.$plane.noenv $rootname/irm_R_bpm.mat.$plane.noenv \
                     -oldColumnNames=ControlName \
                     -newColumnNames=BPMName \
                     -minimum=0.0 \
                     -uMatrix=$rootname//R_bpm.mat.u.$plane.noenv \
                     -sFile=$rootname/R_bpm.mat.s.$plane.noenv \
                     -vMatrix=$rootname/R_bpm.mat.v.$plane.noenv } result] {
            return -code error "Error create irm_R_bpm.mat.$plane.noenv: $result"
        }
        
    }
    
    #compute the whole matrix 32bpms x 8 correctors
    #first make 32bpm x 8 corrector model matrix -- non dialog -zero
    set tmpRoot /tmp/[APSTmpString]
   
    if [catch {eval exec sddsxref $AllDriveList $rootname/${rootname}_CorrectorDriveMatrix.sdds.noenv -take=S* -nowarnings
        eval exec sddsxref $AllErrorList $rootname/${rootname}_CorrectorErrorMatrix.sdds.noenv -take=S* -nowarnings } result] {
        return -code error "Error creating all corrector matrix: $result"
    }
    if ![file exist $rootname/rm.all] {
        if [catch {exec sddsprocess $rootname/Hrm.all $tmpRoot.h \
                     -reedit=col,BPMName,ei/:X/ \
                     -define=col,S27A:V3,0,units=mm/A \
                     -define=col,S27B:V4,0,units=mm/A \
                     -define=col,S28A:V3,0,units=mm/A \
                     -define=col,S28B:V4,0,units=mm/A } result] {
            return -code error "Error creating x bpm matrix: $result"
        }
        if [catch {exec sddsprocess $rootname/Vrm.all -pipe=out \
                     -reedit=col,BPMName,ei/:Y/ \
                     -define=col,S27A:H3,0,units=mm/A \
                     -define=col,S27B:H4,0,units=mm/A \
                     -define=col,S28A:H3,0,units=mm/A \
                     -define=col,S28B:H4,0,units=mm/A \
                     | sddssortcolumn -pipe=in $tmpRoot.v \
                     -sortList=BPMName,[join $corrList ,]} result] {
            return -code error "Error creating y bpm matrix: $result"
        }
        if [catch {exec sddscombine $tmpRoot.h $tmpRoot.v -merge $rootname/rm.all -overwrite } result] {
            return -code error "Error creating ALL rm matrix: $result"
        }
    }
    set files [APSReplicateItem -item $rootname/rm.all -number $npages]
    if [catch {eval exec sddscombine $files $rootname/rm.all.noenv -over} result] {
        return -code error "Error creating ALL rm matrix1: $result"
    }
    #create bpm error full matrix
    APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.h $tmpRoot.v"
    if [catch {exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_H.sdds.noenv -edit=col,S*,%/_BPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.h1 -reedit=col,BPMName,ei/:X/ -redefine=par,Page,i_page,type=long
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_H.sdds.coupling.noenv -edit=col,S*,%/_CouplingBPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.h2 -reedit=col,BPMName,ei/:Y/ -redefine=par,Page,i_page,type=long
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_V.sdds.noenv -edit=col,S*,%/_BPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.v2 -reedit=col,BPMName,ei/:Y/ -redefine=par,Page,i_page,type=long
        exec sddsconvert $rootname/${rootname}_BPMErrorMatrix_V.sdds.coupling.noenv -edit=col,S*,%/_CouplingBPMError// -pipe=out \
                 | sddsprocess -pipe=in $tmpRoot.v1 -reedit=col,BPMName,ei/:X/ -redefine=par,Page,i_page,type=long  } result] {
        return -code error "Error creating full bpm error matrix1: $result"
    }
    APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.h1 $tmpRoot.h2 $tmpRoot.v1 $tmpRoot.v2"
    
    if [catch {exec sddscombine $tmpRoot.h1 $tmpRoot.h2 -pipe=out | sddssort -pipe -param=Page | sddscombine -pipe=in -merge=Page $tmpRoot.hbpm -over
        exec sddscombine $tmpRoot.v1 $tmpRoot.v2 -pipe=out | sddssort -pipe -param=Page | sddscombine -pipe=in -merge=Page $tmpRoot.vbpm -over
        exec sddsxref $tmpRoot.hbpm $tmpRoot.vbpm -take=S* $rootname/${rootname}_BPMErrorMatrix.sdds.noenv } result] {
        return -code error "Error creating full bpm error matrix2: $result"
    }
     APSAddToTmpFileList -ID fdc -fileList "$tmpRoot.hbpm $tmpRoot.vbpm"
    #create full corrector error matrix
    
    #compute R_mat = modelMatrix * corrError * inverse of CorrDrive * -1
    if [catch {exec  sddsmatrixop $rootname/rm.all.noenv -push=$rootname/${rootname}_CorrectorErrorMatrix.sdds.noenv -mult $tmpRoot.1 
        exec sddspseudoinverse $rootname/${rootname}_CorrectorDriveMatrix.sdds.noenv $tmpRoot.2
        exec sddsmatrixop $tmpRoot.1 -push=$tmpRoot.2 -mult -scalarMultiply=-1.0 -pipe=out \
                 | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix.sdds.noenv \
                 -take=BPMName \
                 | sddsconvert -pipe=in $rootname/R.mat.noenv  \
                 -rename=col,doubleColumn0=S27A:H3 \
                 -rename=col,doubleColumn1=S27B:H4 \
                 -rename=col,doubleColumn2=S28A:H3 \
                 -rename=col,doubleColumn3=S28B:H4 \
                 -rename=col,doubleColumn4=S27A:V3 \
                 -rename=col,doubleColumn5=S27B:V4 \
                 -rename=col,doubleColumn6=S28A:V3 \
                 -rename=col,doubleColumn7=S28B:V4 } result] {
        return -code error "Error creating full R.mat matrix: $result"
    }
    if [catch {exec sddsmatrixop -pipe=out $rootname/${rootname}_BPMErrorMatrix.sdds.noenv \
                 -push=$tmpRoot.2 -multiply -scalarMultiply=-1.0 \
                 | sddsxref -pipe $rootname/${rootname}_BPMErrorMatrix.sdds.noenv \
                 -take=BPMName \
                 | sddsconvert -pipe=in $rootname/R_bpm.mat.noenv \
                 -rename=col,doubleColumn0=S27A:H3 \
                 -rename=col,doubleColumn1=S27B:H4 \
                 -rename=col,doubleColumn2=S28A:H3 \
                 -rename=col,doubleColumn3=S28B:H4 \
                 -rename=col,doubleColumn4=S27A:V3 \
                 -rename=col,doubleColumn5=S27B:V4 \
                 -rename=col,doubleColumn6=S28A:V3 \
                 -rename=col,doubleColumn7=S28B:V4} result] {
        return -code error "Error creating full R_bpm.mat: $result"
    }
    #do sddsenvelope for matrices, 
    foreach file {R.mat.H.noenv R.mat.V.noenv R_bpm.mat.H.noenv R_bpm.mat.V.noenv R.mat.noenv R_bpm.mat.noenv} {
        set n [exec sdds2stream -npages=bar $rootname/$file]
        if [catch {exec sddsenvelope $rootname/$file -pipe=out -mean=S* -stand=S* \
                     | sddsxref -pipe -take=BPMName $rootname/$file \
                     | sddsprocess -pipe "-define=col,%sError,%s $n sqrt /,select=*StDev" \
                     | sddsconvert -pipe=in -edit=col,S*,%/Mean//%/StDevError/Error/ $rootname/$file.ave } result] {
            return -code error "Error for averaging matrix: $result"
        }
    }
    PlotNoENVMatrixError
    SetStatus "done."
}

proc PlotNoENVMatrixError {args} {
    global outputDir rootname
    cd $outputDir
    foreach plane {H V} {
        exec sddsplot $outputDir/$rootname/R.mat.$plane.noenv.ave  -layout=2,2 \
           "-topline=$plane R_Corr Matrix with error" \
          -col=BPMName,S27A:${plane}3,%sError -graph=errorbar,con,type=1,sub=1,scale=2 -endp \
          -col=BPMName,S27B:${plane}4,%sError -graph=errorbar,con,type=1,sub=1,scale=2  -endp \
          -col=BPMName,S27B:${plane}4,%sError -graph=errorbar,con,type=1,sub=1,scale=2 -endp \
          -col=BPMName,S28A:${plane}3,%sError -graph=errorbar,con,type=1,sub=1,scale=2 -endp \
          -col=BPMName,S28B:${plane}4,%sError -graph=errorbar,con,type=1,sub=1,scale=2 -endp &
        exec sddsplot $outputDir/$rootname/R_bpm.mat.$plane.noenv.ave  -layout=2,2 \
           "-topline=$plane R_bpm Matrix with error" \
          -col=BPMName,S27A:${plane}3,%sError -graph=errorbar,con -endp \
          -col=BPMName,S27B:${plane}4,%sError -graph=errorbar,con  -endp \
          -col=BPMName,S27B:${plane}4,%sError -graph=errorbar,con -endp \
          -col=BPMName,S28A:${plane}3,%sError -graph=errorbar,con -endp \
          -col=BPMName,S28B:${plane}4,%sError -graph=errorbar,con -endp &

       
    }
}

proc CompareMatrices {args} {
    global outputDir rootname
    cd $outputDir
    foreach plane {H V} {
        exec sddsplot "-topline=$plane plane R_corr matrix of each data set" -leg=par=PageString \
          -layout=2,2 -grap=line,vary -group=nameindex -sep=nameindex -split=page -col=BPMName,S*${plane}? \
          $rootname/R.mat.$plane.noenv &

        exec sddsplot "-topline=$plane plane R_bpm of each data set" -leg=par=PageString  \
          -layout=2,2 -grap=line,vary -group=nameindex -sep=nameindex -split=page -col=BPMName,S*${plane}? \
          $rootname/R_bpm.mat.$plane.noenv &

        exec sddsplot "-topline=$plane plane R_corr matrix comparison"  -layout=2,2 -grap=line,vary -group=nameindex -sep=nameindex -split=page -col=BPMName,S*${plane}? \
          $rootname/R.mat.$plane.noenv.ave -leg=spec=matrix_average  -col=BPMName,S*${plane}?  $rootname/R.mat.$plane -leg=spec=fft_average &

        exec sddsplot "-topline=$plane plane R_bpm matrix comparison"   -layout=2,2 -grap=line,vary -group=nameindex -sep=nameindex -split=page -col=BPMName,S*${plane}? \
          $rootname/R_bpm.mat.$plane.noenv.ave -leg=spec=matrix_average  -col=BPMName,S*${plane}?  $rootname/R_bpm.mat.$plane -leg=spec=fft_average &
    }

}

SetOutputDataDir
set CorrH1Np 38
set CorrH2Np 39
set CorrH3Np 40
set CorrH4Np 41
set CorrV1Np 42
set CorrV2Np 43
set CorrV3Np 44
set CorrV4Np 45

set HCurrentAmp 2.5
set VCurrentAmp 2.5

APSApplication . -name $appName -version $CVSRevisionAuthor \
    -overview { measure FBC response matrix }

APSScrolledStatus .status -parent .userFrame -width 100 -textVariable status

APSLabeledEntry .dir -parent .userFrame -label "Output Directory:" -textVariable outputDir -width 90
APSButton .daily -parent .userFrame.dir -text "daily" -size small -command "SetOutputDataDir"
APSLabeledEntry .model -parent .userFrame -label "FBC matrix dir:" -textVariable modelDir -width 90
APSButton .elegant -parent .userFrame.model -packOption "-side right" -text "elegant" -size small -command "ChangeModelMatrix -type elegant"
APSButton .meas -parent .userFrame.model -packOption "-side right" -text "Prev Meas" -size small -command "ChangeModelMatrix -type prevmeas"
APSFrameGrid .grid -parent .userFrame -xList {x1 x2} -label  "Data Collection Parameters"
set w1 .userFrame.grid.x1
set w2 .userFrame.grid.x2
APSLabeledEntry .hmodel -parent $w1 -label "H FBC dir:" -width 25 -textVariable HModelDir
APSLabeledEntry .vmodel -parent $w2 -label "V FBC dir:" -width 25 -textVariable VModelDir
APSLabeledEntry .interval -parent $w1 -label "Interval (seconds):" -width 25 -textVariable interval
APSLabeledEntry .steps -parent $w2 -label "Steps:" -width 25 -textVariable steps
APSLabeledEntry .current -parent $w1 -label "H Corrector current(A)" -width 25 -textVariable HCurrentAmp
APSLabeledEntry .vcurrent -parent $w2 -label "V Corrector current(A)" -width 25 -textVariable VCurrentAmp
APSLabeledEntryFrame .corr1 -parent $w1 -label "H Corrector Np" -variableList {CorrH1Np CorrH2Np CorrH3Np CorrH4Np} -width 10 \
    -orientation horizontal
APSLabeledEntryFrame .corr2 -parent $w2 -label "V Corrector Np" -variableList {CorrV1Np CorrV2Np CorrV3Np CorrV4Np} -width 10 \
    -orientation horizontal

APSLabeledEntry .root -parent $w1 -label "Rootname:" -textVariable rootname -width 25
bind $w1.root.entry <Return> ChangeModelMatrix
#APSLabeledEntry .freq -parent $w1 -label "Perturb frequency (Hz):" -width 25 -textVariable freq
#APSButton .compute -parent $w1.freq -text "Update" -size small -command "ComputeFrequency"
#APSLabeledEntry .index -parent $w2 -label "Index" -width 25 -textVariable index -contextHelp "give the index of response matrix measurement." 
set irmLoad irm_R

APSRadioButtonFrame .irmload -parent $w2 -label "irm to load:" -buttonList {irm_R(corr) irm_R_bpm(bpm)} -valueList {irm_R irm_R_bpm} \
  -variable irmLoad -orientation horizontal
set rootname testKp0Ki0

#APSButton .get -parent $w1.root -packOption "-side right" -text "update" -size small -command "set rootname [clock format [clock seconds] -format %Y-%m%d:%H%M%S]"
#APSButton .f -parent $w1.root -packOption "-side right" -text "F" -size small -command "GetRootname"



#APSRadioButtonFrame .afg -parent $w2 -label "AFG waveform:" -buttonList {AFG1 AFG2 AFG3 AFG4} -valueList {AFG1 AFG2 AFG3 AFG4} -variable AFG -orientation horizontal
#set S27A3 1
#set S27B4 1
#set S28A3 1
#set S28B4 1
#APSCheckButtonFrame .corr -parent .userFrame -label "Select correctors:" -buttonList {S27A:\[HV\]3 S27B:\[HV\]4 S28A:\[HV\]3 S28B:\[HV\]4} \
#  -variableList {S27A3 S27B4 S28A3 S28B4} -allNone 1 -orientation horizontal

APSFrame .f1 -parent .userFrame 
APSFrame .f2 -parent .userFrame
.userFrame.f1.frame configure -bd 0
.userFrame.f2.frame configure -bd 0
set w1 .userFrame.f1.frame
set w2 .userFrame.f2.frame

APSButton .freq -parent $w1 -text "Generate AFG" -command "GenerateAFGDriveWaveform"
APSButton .afg -parent $w1 -text "Load AFG" -command "LoadAFG"
APSButton .start -parent $w1 -text "Start measurement" -command "Start"
APSButton .load -parent $w1 -text "Load IRM" -command "LoadIRM -plot 1 -load 1"
APSButton .process -parent $w1 -text "Process Data" -command "ProcessData1"
APSButton .compute -parent $w1 -text "Compute Matrix" -command "ComputeResponseMatrix -plot 1"
APSButton .plotmatrix -parent $w1 -text "Plot Matrix" -command "PlotMatrix"
APSButton .checkmatrix -parent $w1 -text "Check FBC IRM" -command "LoadIRM -load 0 -plot 1"
APSButton .take -parent $w1 -text "CollectDataOnly" -command "Start -takeDataOnly 1"
APSButton .process1 -parent $w2 -text "Process Data (no average)" -command "ProcessDataNoAverage"
APSButton .compute1 -parent $w2 -text "Compute Matrix(for each data set)" -command "ComputeResponseMatrixForEachDataSet"
APSButton .plot1 -parent $w2 -text "PlotMatrixError" -command "PlotNoENVMatrixError"
APSButton .plot2 -parent $w2 -text "CompareMatrices" -command "CompareMatrices"
APSButton .plot3 -parent $w2 -text "PlotRawData" -command "PlotRawData"
APSButton .plot4 -parent $w2 -text "PlotFFTData" -command "PlotFFTData"
