%compares orbit diffs with MAD 
clear all;
close all;

fromkicker = 3; %which kicker
tokicker = fromkicker;

dir = '/home/skowron/ctf3emitdata/'; %directory with data files
%dir = '/mnt/psop/Psop/Archives/data/CTF3/2007/week25/ResponseMtxMeas/';

homedir = '/mnt/ctfop/Public/ctf3emit'; %dir were we start and we want to end
modelhome = '/home/skowron/cern/ctfmod/cr'; %dir with the madx model

%time stamps of the data sets: each deta set contains data for different values of kicks
%  Normal optics
measureset=['7332365434';%1A
            '7332365369';%-1A
            '7332365285';                                 
            '7332365196';                                 
            '7332365109';%0.5A                                 
            '7332365079'];


%   Weaker optics
measureset=['7332367735';
            '7332367816';
            '7332367828';
            '7332367912';
            '7332367807';
            '7332367749'];


[nds,tmpd] = size(measureset);

fprintf(1,'%d %d\n',nds,tmpd);

%f1 = figure (1);
f2 = figure (2);
f3 = figure (3);

lastkick = 1.0;

theorycalc = 0;
nset = 1;

avgxdif = zeros(1,1); %array of measurements for different kickes, not used here, only filled
                      % after the script is completed one can get the averaged normalized orbit 

for ds=1:nds, %loop over available data sets
  
  timestamp = measureset(ds,:);

  datfname = [dir 'bpx'      timestamp '.dat']; %file name with data
  curfname = [dir 'currents' timestamp '.dat']; %file name with the currents settings

  %read the data
  [nkickersread, mk, m, rm_measured, reforbits, kicks, Corrector, Monitor ] = BPXread_lx(datfname)

  %check the size of the responce matrix for this data set
  [nkicks,nmons]  = size(rm_measured);


  fprintf(1,'Array has size %d %d\n', nkicks,nmons);
  fprintf(1,'There is %d kicks\n', length(kicks) );

  %go to online model directory (containing the MADX script)
  cd ('/home/skowron/cern/ctfmod/cr');
  
  %copy the corresponding currents file that contains quad currents during the measurement
  cpycmd=['cp ' curfname ' currents '];

  system(cpycmd);
  fpo=fopen('currents','a');
  fprintf(fpo,'EN=118;\n'); %energy was not stored so we need to put some
  fclose(fpo);
  cd(homedir);


  % normaly we run only one kicker, legace loop
  for i=1:nkicks,

    fprintf(1,'---\n');
    idxcorr = mk(i);

    if (kicks(i) == 0)
      fprintf(1,'Was not kicked\n');
      continue;
    end
    
    if ( (idxcorr < fromkicker) || (idxcorr > tokicker) ) 
      continue;
    end
    
    fprintf(1,'KickerNo %d named %s set to %f;\n',i,Corrector(idxcorr).name,kicks(i));

    
    %calculates the responce from the model
    if (theorycalc == 0) %+=+=+=

      cd (modelhome);
      fpo=fopen('setcorrs','w');
      icorrvar = Corrector(idxcorr).madname;
      fprintf(1,'Corrector name %s current var name %s\n', Corrector(idxcorr).name, icorrvar);
      fprintf(fpo,'%s = %f;\n', icorrvar ,kicks(i)); %produces MAD input file that defines the kick
      fclose(fpo);
    
      system('madxp < kickedorbit.madx > out.txt'); %run the model
    %  fpo=fopen('xorbit','r');
    %  fclose(fpo);

      twissdata = importdata('xorbit',' ',47); %import the calculated orbit

      cd(homedir);

      %normally it is enough to calculate the orbit for one kick
      % however if we are not sure that the machine settings were the same for all the measurements 
      % then comment the lines marked with +=+=+=
      theorycalc = 1; %+=+=+=
    end               %+=+=+=



    ltext = length(twissdata.textdata);
    [nrow,ncol] = size(twissdata.data);

    fprintf(1,'Imported %d text rows, %d x %d numbers\n', ltext, nrow, ncol);

    xdiffs = zeros(nmons,1) %difference between measurement and theory
    xdiffs1 = zeros(nmons,1)
    xdiffs2 = zeros(nrow,1)

    xdiffs2save = zeros(nrow,1)

    spos    = zeros(nmons,1)
    spos2   = zeros(nrow,1)
    kk = 1;
    for j=1:nrow,

      idxmon = m(j);

      xdiffs2(j) =  1000.0*twissdata.data(j,2) ; %theory orbit in mm
      %spos2(j)   = twissdata.data(j,1);  %s pos in meters
      spos2(j)   = twissdata.data(j,3); %phase advance

      if (j > nmons)
        continue;
      end

      if (j == 6) %in the present data it seems that the 6th monitor was badly scaled
        continue; 
      end

      if (reforbits(i,j) == 0)
        continue;
      end

  %    fprintf(1,'BPM %d idx %d\n',j,idxmon);
  %    fprintf(1,'\t DB    %s %f \n', Monitor(idxmon).name, Monitor(idxmon).s );

      madxname = char(twissdata.textdata(ltext - nrow + idxmon));
      madxpos =  twissdata.data(idxmon,1);

  %    fprintf(1,'\t MADX  %s %f \n', madxname , madxpos );
  %    fprintf(1,'measured diff %f predicted %f \n', rm_measured(i,j), 1000.0*twissdata.data(idxmon,2));

      xdiffs(kk) =  rm_measured(i,j) - 1000.0*twissdata.data(idxmon,2);

      xdiffs1(kk) =  rm_measured(i,j) / kicks(i); %normalized kicked orbit
      avgxdif(nset, kk) = rm_measured(i,j) / kicks(i);

      %spos(kk) = twissdata.data(idxmon,1); %s pos in meters
      spos(kk) = twissdata.data(idxmon,3); %phase advance
      

      kk = kk + 1;
    end

    if ( kk < 2) %arrays above were not filled in because of missing data for the requested BPMs
      continue;
    end
    
%    figure (f1);
%    hold all;
%    plot(spos(1:kk-1),xdiffs(1:kk-1),':o','LineWidth',2.);
    
    
    figure (f2);
    hold all;
    h = plot(spos(1:kk-1),xdiffs1(1:kk-1),':o','LineWidth',2.);
%    errors = 1.0*ones(kk-1) / kicks(i);%we assume error of 1mm
%    h = errorbar(spos(1:kk-1),xdiffs1(1:kk-1),errors,':o','LineWidth',2.);
    hold all;
    legend(h,[timestamp sprintf('%+4.2i',kicks(i))]);
    hold all;
%    plot(spos2,xdiffs2,':o','LineWidth',2.);
%    hold all;


    figure (f3);
    hold all;
    plot(spos2,xdiffs2,':o','LineWidth',.1);
    legend(timestamp);
    xdiffs2 = xdiffs2 / kicks(i);
    hold all;
    plot(spos2,xdiffs2,'-o','LineWidth',2.);
    legend(timestamp);
    hold all;
    
    cd(homedir); %
    xdiffs2save = xdiffs2;
    
    nset = nset + 1;
    
  end

end

figure (f2);
hold all;
xdiffs2 = 1.0* xdiffs2save;
plot(spos2,xdiffs2,'-o','LineWidth',2.);
xlabel('\mu')
ylabel('Normalized offset [mm]')

return;

set(gca,'XTick',spos2)
set(gca,'XTickLabel',{'BPI0130', 'BPM0155', 'BPM0195', 'BPI0208', 'BPI0248', 'BPI0275', ...
                      'BPI0305', 'BPI0395', 'BPI0425', 'BPI0448', 'BPI0475', 'BPR0505', ...
	  'BPI0570', 'BPI0610', 'BPM0650', 'BPM0695', 'BPI0708', 'BPI0748', ...
	  'BPI0775', 'BPI0805', 'BPI0895', 'BPI0925', 'BPI0952', 'BPI0992', ...
                      'BPM1025', 'BPI1070'})
  

