function [stat xlim ylim rotlim] = FlSetMoverLimits(igir)
% FLSETMOVERLIMITS
%   Set dynamic high/low limits for given Mover (provide GIRDER index igir)
global GIRDER FL BEAMLINE
persistent camsettings

stat{1}=1;

% Mover CAM constants
if isempty(camsettings)
  camsettings.quad.a = 0.145250;
  camsettings.quad.c = 0.123952;
  camsettings.quad.R = 0.031;
  camsettings.quad.L = 0.0015875;
  camsettings.quad.S1 = 0.034925;
  camsettings.quad.S2 = 0.2905;
  camsettings.quad.b = camsettings.quad.c + ...
    camsettings.quad.S1 - (sqrt(2)*camsettings.quad.R);
  camsettings.sext.a = 0.145250;
  camsettings.sext.c = 0.123952;
  camsettings.sext.R = 0.031;
  camsettings.sext.L = 0.0015875;
  camsettings.sext.S1 = 0.034925;
  camsettings.sext.S2 = 0.2905;
  camsettings.sext.b = camsettings.sext.c + ...
    camsettings.sext.S1 - (sqrt(2)*camsettings.sext.R);
end

% Argument check
igir=round(igir);
if igir<1 || igir>length(GIRDER) || ~isfield(GIRDER{igir},'Mover') 
  stat{1}=-1;
  stat{2}='No MOVER on this GIRDER or invalid index';
  return
end
  
% Get Mover location
xmov = GIRDER{igir}.MoverSetPt(1);
ymov = GIRDER{igir}.MoverSetPt(2);
rotmov = GIRDER{igir}.MoverSetPt(3);
% betaminus = pi/4 - rotmov;
% betaplus  = pi/4 + rotmov;

% Get magnet type on Mover
camset=[];
for ele=GIRDER{igir}.Element(1):GIRDER{igir}.Element(end)
  if strcmp(BEAMLINE{ele}.Class,'QUAD')
    camset=camsettings.quad;
    break
  elseif strcmp(BEAMLINE{ele}.Class,'SEXT')
    camset=camsettings.sext;
    break
  end
end
if isempty(camset)
  stat{1}=-1;
  stat{2}='Magnet type on GIRDER not QUAD or SEXT';
  return
end

% x_1 = xmov + (camset.a*cos(rotmov)) + (camset.b*sin(rotmov)) - camset.a;
% y_1 = ymov - (camset.b*cos(rotmov)) + (camset.a*sin(rotmov)) + camset.c;

% Compute x limits
% x1max(1) = ((camset.L + y_1*cos(rotmov) - camset.c + camset.b) / sin(rotmov)) - camset.S2 ;
% x1max(2) = ((camset.L - y_1*cos(betaminus) + camset.R) / sin(betaminus)) - camset.S1 ;
% x1max(3) = ((camset.L + y_1*cos(betaplus) - camset.R) / sin(betaplus)) - camset.S1 ;
% xmax = min(abs(x1max - camset.a*cos(rotmov) - camset.b*sin(rotmov) + camset.a));
xlim=[-2.5e-3 2.5e-3];
for xtry=0:-1e-5:-2.5e-3
  camangles=camcalc(xtry,ymov,rotmov,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    xlim(1)=xtry;
    break
  end
end
for xtry=0:1e-5:2.5e-3
  camangles=camcalc(xtry,ymov,rotmov,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    xlim(2)=xtry;
    break
  end
end

% Compute y limits
% y1max(1) = ((x_1+camset.S2)*sin(rotmov) - camset.L + camset.c - camset.b) / cos(rotmov) ;
% y1max(2) = (camset.L - (x_1+camset.S1)*sin(betaminus) + camset.R ) / cos(betaminus) ;
% y1max(3) = ((x_1-camset.S1)*sin(betaplus) + camset.R - camset.L) / cos(betaplus) ;
% ymax = min(abs(y1max + camset.b*cos(rotmov) - camset.a*sin(rotmov) - camset.c)) ;
ylim=[-2e-3 2e-3];
for ytry=0:-1e-5:-2e-3
  camangles=camcalc(xmov,ytry,rotmov,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    ylim(1)=ytry;
    break
  end
end
for ytry=0:1e-5:2e-3
  camangles=camcalc(xmov,ytry,rotmov,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    ylim(2)=ytry;
    break
  end
end

% Compute roll limits
% rotmax(1) = abs(fminbnd(@(x) minth1(x,camset,xmov,ymov),-0.03,0.03,optimset('TolX',1e-6,'TolFun',1e-12,'Display','off')));
% rotmax(2) = abs(fminbnd(@(x) minth2(x,camset,xmov,ymov),-0.03,0.03,optimset('TolX',1e-6,'TolFun',1e-12,'Display','off')));
% rotmax(3) = abs(fminbnd(@(x) minth3(x,camset,xmov,ymov),-0.03,0.03,optimset('TolX',1e-6,'TolFun',1e-12,'Display','off')));
% rotmax = min(rotmax) ;
rotlim=[-10e-3 10e-3];
for ttry=0:-10e-6:-10e-3
  camangles=camcalc(xmov,ymov,ttry,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    rotlim(1)=ttry;
    break
  end
end
for ttry=0:10e-6:10e-3
  camangles=camcalc(xmov,ymov,ttry,camset);
  if ~isreal(camangles) || any(abs(camangles)>pi/2)
    rotlim(2)=ttry;
    break
  end
end

% Set Limits
FL.HwInfo.GIRDER(igir).low=[xlim(1) ylim(1) rotlim(1)];
FL.HwInfo.GIRDER(igir).high=[xlim(2) ylim(2) rotlim(2)];

% function chi2 = minth1(th,c,x,y)
% x1 = x + (c.a*cos(th)) + (c.b*sin(th)) - c.a;
% y1 = y - (c.b*cos(th)) + (c.a*sin(th)) + c.c;
% chi2=(1/c.L) * ( (x1 + c.S2)*sin(th) - y1*cos(th) + c.c - c.b ) - 1 ;
% chi2 = chi2^2;
% 
% function chi2 = minth2(th,c,x,y)
% x1 = x + (c.a*cos(th)) + (c.b*sin(th)) - c.a;
% y1 = y - (c.b*cos(th)) + (c.a*sin(th)) + c.c;
% betaminus = pi/4 - th;
% chi2=(1/c.L) * ( (x1+c.S1)*sin(betaminus) + y1*cos(betaminus) - c.R ) - 1 ;
% chi2=chi2^2;
% 
% function chi2 = minth3(th,c,x,y)
% x1 = x + (c.a*cos(th)) + (c.b*sin(th)) - c.a;
% y1 = y - (c.b*cos(th)) + (c.a*sin(th)) + c.c;
% betaplus  = pi/4 + th;
% chi2 = (1/c.L) * ( (x1 - c.S1)*sin(betaplus) - y1*cos(betaplus) + c.R ) - 1 ;
% chi2=chi2^2;

function camangles=camcalc(xmov,ymov,rotmov,camset)
betaminus = pi/4 - rotmov;
betaplus  = pi/4 + rotmov;
x_1 = xmov + (camset.a*cos(rotmov)) + (camset.b*sin(rotmov)) - camset.a;
y_1 = ymov - (camset.b*cos(rotmov)) + (camset.a*sin(rotmov)) + camset.c;
camangles(1) = rotmov - asin((1/camset.L) * ((x_1+camset.S2)*sin(rotmov) - y_1*cos(rotmov) + (camset.c - camset.b)));
camangles(2) = rotmov - asin((1/camset.L) * ((x_1+camset.S1)*sin(betaminus) + y_1*cos(betaminus) - camset.R));
camangles(3) = rotmov - asin((1/camset.L) * ((x_1-camset.S1)*sin(betaplus) - y_1*cos(betaplus) + camset.R));