% Script that measures dispersion and compares with the model
% Piotr Skowronski (CERN)

close all;
clear all;
clc;

global testmode  ncalls  scalings

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Test flag -> if true the orbit is simulated
testmode = 0;
ncalls = 0;   %FOR	offline tests
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Scaling factors in percents
scalings = [0, 1, -0.5, 0.5, 1]; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Flag sayint to scale or not also correctors
scalecorrectors = 1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Number of measurments for averaging
navg = 10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Debug flagt
debug = 0;



nmeas = length(scalings);
fprintf(1,'Number of scalings %d\n', nmeas);




[ctfmodroot, dataout, madxp] = ctfsettings();
addpath([ctfmodroot '/MatLab/JavaCoInterface']);



currentdir = pwd;
fprintf(1,'Current dir is %s\n',currentdir);
addpath(currentdir);


timestepf = now*10000;
dx_filnam=[dataout '/dx' sprintf('%10.0f',timestepf) '.dat'];
dy_filnam=[dataout '/dy' sprintf('%10.0f',timestepf) '.dat'];
di_filnam=[dataout '/di' sprintf('%10.0f',timestepf) '.dat'];

dxe_filnam=[dataout '/dx' sprintf('%10.0f',timestepf) '.rms.dat'];
dye_filnam=[dataout '/dy' sprintf('%10.0f',timestepf) '.rms.dat'];
die_filnam=[dataout '/di' sprintf('%10.0f',timestepf) '.rms.dat'];

dc_filnam=[dataout '/dcurrents' sprintf('%10.0f',timestepf) '.dat'];



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%      Get definitions of devices        %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


sectionname = 'cr';

fprintf(1,'############################################3\n');
fprintf(1,'############################################3\n');
fprintf(1,'###  Section Name: %s\n', sectionname);
fprintf(1,'############################################3\n');
fprintf(1,'############################################3\n');


switch sectionname
  case 'tl1'
    [prefix, quads, dipols, hcors, vcors, mons] = tl1();
  case 'cr'
    [prefix, quads, dipols, hcors, vcors, mons] = cr();
  case 'ct-dl'  %we scale only dl magnets from injection to ejection
    [prefix, quads, dipols, hcors, vcors, mons] = dl();
end





nquads  = length(quads);
ndipols = length(dipols);
nhcors   = length(hcors);
nvcors   = length(vcors);
nmons   = length(mons);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%    Read current setting of devices     %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


tic;
if (debug) fprintf(1,'Reading Dipoles\n'); end;
dipolsiniv = readdevices(dipols, [prefix '.']);
if (debug) fprintf(1,'Reading Quads\n'); end;
quadsiniv  = readdevices(quads,  [prefix '.']);
if (debug) fprintf(1,'Reading Horizontal Correctors\n'); end;
hcorsiniv  = readdevices(hcors,  [prefix '.']);
if (debug) fprintf(1,'Reading Vertical Correctors\n'); end;
vcorsiniv  = readdevices(vcors,  [prefix '.']);

t = toc;

fprintf(1,'Read all devices within %f seconds\n',t);

fprintf(1,'Producing currents file ....\n'); %energy was not stored so we need to put some
dcf=fopen(dc_filnam,'w');
writecurrentsfile(dcf, dipols, dipolsiniv);
writecurrentsfile(dcf, quads,  quadsiniv);
fclose(dcf);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%        Get model dispersion            %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fcbendcurrent =  Jgetcontrolval('CT.BHE0210-S','CCV');
fprintf(1,'Frascatti Chicane Bend Current is %f\n',fcbendcurrent);

EN = abs(fcbendcurrent)*1.408;

fprintf(1,'Corresponding Energy %f\n',EN);


cd([ ctfmodroot '/' sectionname]);

dcf=fopen('currents','w');
if (dcf < 0)
 error('Can not open currents file');
 return;
end

writecurrentsfile(dcf, dipols, dipolsiniv);
writecurrentsfile(dcf, quads,quadsiniv);

fprintf(dcf,'EN = %f ;\n',EN);
fclose(dcf);

if (exist('dispersion.twiss','file')) delete('dispersion.twiss'); end;

cmnd = [ madxp ' < dispersion.madx '  ];
cmnd = [ madxp ' < dispersion.madx > dispersion.out'  ];
fprintf(1,'Running MADX....\n%s\n',cmnd); %energy was not stored so we need to put some

system(cmnd);

twissdata = importdata('dispersion.twiss',' ',47); %import the calculated pattern

cd(currentdir);


figure(1)
h = plot(twissdata.data(:,1),twissdata.data(:,2),'-o','LineWidth',3.);
hold all;
legend(h,'Model Dispersion');
hold all;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Pickup's positions form MAD %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

spos  = zeros(1,1);
thpat = zeros(1,1);

k = 0;
for i=1:length(mons)
  if (mons(i).faulty)
    continue;
  end
  k = k + 1;
  
  dbname = [prefix '.' mons(i).name];
  
  madxname = '###';
  j=1;
  for j=(ltext - nrow):ltext
    madxname = char(twissdata.textdata(j));
    
    if (strfind(madxname, dbname))
      break;
    end
    madxname = '###';  
  end

  if (strcmp(madxname,'###'))
    error('Can not find monitor %s present in DB in MAD result table', dbname);
    return;
  end

  spos(k,1)  =  twissdata.data(j - (ltext - nrow),1);
  thpat(k,1) =  twissdata.data(j - (ltext - nrow),2);
  
  fprintf(1,'%d: DB %s @ %f       MAD  %s @ %f \n',i,mons(i).name,mons(i).spos, madxname, spos(k));
end


global spos;
global thpat;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%      Read the reference orbit          %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global referenceH  referenceV  referenceC

[referenceH, referenceV, referenceC, spos] = readorbit(mons,[prefix '.'], navg);


figure(2)
%plot with errors bars
hold all;
h = errorbar(spos,referenceH(:,1),referenceH(:,2),'-o','LineWidth',3.);
hold all;
legend(h,'Reference Orbit');
hold all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%       Scale  and  Read orbits          %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




pattH = zeros(k,nmeas);
pattV = zeros(k,nmeas);
pattC = zeros(k,nmeas);
pattHerr = zeros(k,nmeas);
pattVerr = zeros(k,nmeas);
pattCerr = zeros(k,nmeas);

for i=1:nmeas

  fprintf(1,'Start of measurement %d of %d \n',i, nmeas);
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%%%   Scale CCV values    %%%%%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  dipolsv = dipolsiniv * (1.0+scalings(i)/100.0);
  quadsv  = quadsiniv  * (1.0+scalings(i)/100.0);
  hcorsv   = hcorsiniv   * (1.0+scalings(i)/100.0); 
  vcorsv   = vcorsiniv   * (1.0+scalings(i)/100.0); 
  
  quadsv
  quadsiniv

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%%% Write them to devices %%%%%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  tic;
  writedevices(dipols, [prefix '.'], dipolsv);
  writedevices(quads,  [prefix '.'], quadsv);
  writedevices(hcors,  [prefix '.'], hcorsv);
  writedevices(vcors,  [prefix '.'], vcorsv);
  t = toc;
  fprintf(1,'Wrote all devices within %f seconds\n',t);
  
  pause(3);
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%%%       Read orbit      %%%%%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  fprintf(1,'Reading orbit %d \n',i);
  [orbitH, orbitV, orbitC, spos] = readorbit(mons,[prefix '.'], navg);
  fprintf(1,'Got orbit %d \n',i);


  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%%%       Plot orbit      %%%%%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  figure(2)
  %plot with errors bars
  hold all;
  h = errorbar(spos,orbitH(:,1),orbitH(:,2),'-o','LineWidth',3.);
  hold all;
  legend(h,['Scaled ' sprintf('%+4.2g',scalings(i))]);
  hold all;


  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%%%       Save orbit      %%%%%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  pattH(:,i) = (orbitH(:,1) - referenceH(:,1)); 
  pattV(:,i) = (orbitV(:,1) - referenceV(:,1));
  pattC(:,i) =  orbitC(:,1)
  
  pattHerr(:,i) = hypot(orbitH(:,2),referenceH(:,2));
  pattVerr(:,i) = hypot(orbitV(:,2),referenceV(:,2));
  pattCerr(:,i) =       orbitC(:,2);
  
  fprintf(1,'End of measurement %d of %d\n',i, nmeas);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%    Set initial settings to devices     %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



writedevices(dipols, [prefix '.'], dipolsiniv);
writedevices(quads,  [prefix '.'], quadsiniv);
writedevices(hcors,  [prefix '.'], hcorsiniv);
writedevices(vcors,  [prefix '.'], vcorsiniv);

pause(3);

dipolsv = readdevices(dipols, [prefix '.']);
quadsv  = readdevices(quads,  [prefix '.']);
hcorsv  = readdevices(hcors,  [prefix '.']);
vcorsv  = readdevices(vcors,  [prefix '.']);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%      Write Output files                %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




writeoutfiles(dx_filnam,mons,scalings,referenceH(:,1),pattH);
writeoutfiles(dy_filnam,mons,scalings,referenceV(:,1),pattV);
writeoutfiles(di_filnam,mons,scalings,referenceC(:,1),pattC);

writeoutfiles(dxe_filnam,mons,scalings,referenceH(:,2),pattHerr);
writeoutfiles(dye_filnam,mons,scalings,referenceV(:,2),pattVerr);
writeoutfiles(die_filnam,mons,scalings,referenceC(:,2),pattCerr);



if (scalings(1) == 0 )
  
  fprintf(1,'The first scaling factor is ZERO, it was TEST run, can not proceed further.\n');
  return;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%  Normalize and plot the measured data  %%%%%%%%%
%%%%%%%                                        %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:nmeas

  pattH(:,i) = pattH(:,i) / scalings(i); 
  pattV(:,i) = pattV(:,i) / scalings(i);
  
  pattHerr(:,i) = pattHerr(:,i) / scalings(i);
  pattVerr(:,i) = pattHerr(:,i) / scalings(i);

%  figure(1)
%  hold all;
%  h = errorbar(spos,pattH(:,i),pattHerr(:,i),':o','LineWidth',1.);
%  hold all;
%  legend(h,['Dataset' sprintf(' %4.2g',scalings(i))]);
%  hold all;

  
end




pattHavg = mean(pattH,2);
pattVavg = mean(pattV,2);

pattHavgerr = zeros(1,1);
pattVavgerr = zeros(1,1);


for j=1:length(pattHavg)
  pattHavgerr(j,1) = norm(pattHerr(j,:));
end





for i=1:k
  if (abs(thpat(i)) > 0.5)
    break;
  end
end


scalefactor = thpat(i)/pattHavg(i);

fprintf(1,'Scale factor is %f\n',scalefactor);

pattH    = pattH    * scalefactor;
pattV    = pattV    * scalefactor;
pattHerr = pattHerr * scalefactor;
pattVerr = pattVerr * scalefactor;

pattHavg = pattHavg * scalefactor;
pattVavg = pattVavg * scalefactor;

pattHavgerr = pattHavgerr * scalefactor;
pattVavgerr = pattVavgerr * scalefactor;



for i=1:nmeas

  figure(1)
  hold all;
  h = errorbar(spos,pattH(:,i),pattHerr(:,i),':o','LineWidth',1.);
  hold all;
  legend(h,['Dataset' sprintf(' %4.2g',scalings(i))]);
  hold all;
  
end



figure(1)
hold all;
h = errorbar(spos,pattHavg(:,1),pattHavgerr(:,1),'-o','LineWidth',3.);
hold all;
legend(h,'Average measurement');
hold all;


return;




figure(1)
hold all;
h = errorbar(spos,referenceH(:,1),referenceH(:,2),'-o','LineWidth',3.);
hold all;
legend(h,'Reference Orbit');
hold all;

