% ND 2016/1/11
% Moves the translation stage and all the rotations at the same time

function moveSPESO_Tx_Ts_Tz(targetPositionX,targetPositionS,targetPositionZ, targetPositionRS1, targetPositionRS2, targetPositionRS3, targetPositionRS4)

if (targetPositionZ<-150000)
    error('Position Z is not acceptable')
end

%TZpos =   [   0 -10000 -20000 -30000 -40000 -50000 -60000 -68000 -80000 -90000 -100000 -110000 -120000 -130000 -140000 ];
%RS3pos =  [  25     25     25     20     16     12      7      0     -5    -11     -16     -22     -25     -25     -25  ];
%RS4pos =  [ -35    -35    -27    -20    -18    -12     -6      0      6     12      17      23      28      33      39  ];

% Changed 8/2/2016
TZpos =   [   0 -10000 -20000 -30000 -40000 -50000 -60000 -68000 -80000 -90000 -100000 -110000 -120000 -130000 -140000 ];
RS3pos =  [  25     25     25     20     16     12      7      0     -5    -11     -22     -22     -25     -25     -25  ];
RS4pos =  [ -35    -35    -27    -20    -18    -12     -6      0      6     12      22      23      28      33      39  ];


%%Ref positions
if (targetPositionX==-200000)
%TSpos =   [ 100 -100000 -200000 -300000 -400000 -500000 -600000 -700000 -795311  ];
%RS2pos =  [  90      80      72      65      53      43      34      25      21  ];
%RS1pos =  [   8      10       3      -3     -10     -18     -23     -34     -42  ];
TSpos =   [ 100 -300000 -400000 -500000 -600000 -700000 -795311  ];
RS1pos =  [  50      50      41      30      30      26      25  ];
RS2pos =  [  27      27      23      21      9       3        1  ];
RS3pos = RS3pos * 0.8;
RS4pos = RS4pos * 0.8;
elseif (targetPositionX==-400000)

%Pos before 8/2/2016
%TSpos =   [ 100 -200000 -400000 -600000 -795311  ];
%RS1pos =  [  -3      -3     -15     -27     -40  ];
%RS2pos =  [  80      66      50      37      23  ];

TSpos =   [ 100 -300000 -400000 -500000 -600000 -700000 -795311  ];
RS1pos =  [  50      50      41      30      30      26      25  ];
RS2pos =  [  27      27      23      21      9       3        1  ];
RS3pos = RS3pos * 0.5;
RS4pos = RS4pos * 0.5;
else
    error('Position X is not acceptable')
end % if     




% Definition device translation X et attributs
devRobotTransX=['LT1/DG/','ROBOT-SPESO.MT_Tx.1'];
attr_list_TransX={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device translation Y et attributs
devRobotTransY=['LT1/DG/','ROBOT-SPESO.MT_Ts.1'];
attr_list_TransY={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device translation Z et attributs
devRobotTransZ=['LT1/DG/','ROBOT-SPESO.MT_Tz.1'];
attr_list_TransZ={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device rotation RS1 et attributs
devRobotRS1=['LT1/DG/','ROBOT-SPESO.MT_Rs.1'];
attr_list_RS1={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device rotation RS2 (gentec) et attributs
devRobotRS2=['LT1/DG/','ROBOT-SPESO.MT_Rs.2'];
attr_list_RS2={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device rotation RS3 et attributs
devRobotRS3=['LT1/DG/','ROBOT-SPESO.MT_Rs.3'];
attr_list_RS3={'position','State','backwardLimitSwitch','forwardLimitSwitch'};

% Definition device rotation RS4 et attributs
devRobotRS4=['LT1/DG/','ROBOT-SPESO.MT_Rs.4'];
attr_list_RS4={'position','State','backwardLimitSwitch','forwardLimitSwitch'};





%calculate target position TS
% iinf=find(TSpos<targetPositionS,1);
% isup=iinf-1;

% fracsup=-1*(TSpos(iinf)-targetPositionS)/(TSpos(isup)-TSpos(iinf));
% fracinf=(TSpos(isup)-targetPositionS)/(TSpos(isup)-TSpos(iinf));


% targetPositionTS=(TSpos(iinf)*fracinf)+(TSpos(isup)*fracsup)
% targetPositionRS1=(RS1pos(iinf)*fracinf)+(RS1pos(isup)*fracsup);
% targetPositionRS2=(RS2pos(iinf)*fracinf)+(RS2pos(isup)*fracsup);

targetRS1=targetPositionRS1
targetRS2=targetPositionRS2


%calculate target position TZ
% iinf=find(TZpos<targetPositionZ,1);
% isup=iinf-1;

% fracsup=-1*(TZpos(iinf)-targetPositionZ)/(TZpos(isup)-TZpos(iinf));
% fracinf=(TZpos(isup)-targetPositionZ)/(TZpos(isup)-TZpos(iinf));


% targetPositionTZ=(TZpos(iinf)*fracinf)+(TZpos(isup)*fracsup)
% targetPositionRS3=(RS3pos(iinf)*fracinf)+(RS3pos(isup)*fracsup);
% targetPositionRS4=(RS4pos(iinf)*fracinf)+(RS4pos(isup)*fracsup);

targetRS3=targetPositionRS3
targetRS4=targetPositionRS4


%    error([ 'The difference between RS1 (' num2str(targetRS1) ' and RS2 (' num2str(targetRS2) ') is too large!!! ' ]);
%end % check the difference between the two detectors

%if (abs(targetRS1-targetRS2)>45)
%    error([ 'The difference between RS1 (' num2str(targetRS1) ' and RS2 (' num2str(targetRS2) ') is too large!!! ' ]);
%end % check the difference between the two detectors


%Checking that positions are safe
if (targetRS3<-25)
    disp(' Target RS3 should not be lower than -25');
    targetRS3=-25;
end %if
if (targetRS3>25)
    disp(' Target RS3 should not be greater than 25');
    targetRS3=25;
end %if

if (targetRS4<-40)
    disp(' Target RS4 should not be lower than -40');
    targetRS4=-40;
end %if
if (targetRS4>50)
    disp(' Target RS4 should not be greater than 50');
    targetRS4=50;
end %if


% Start TX movement
tango_write_attribute2(devRobotTransX,'position',targetPositionX);
pause(0.2)

% Start TS movement
tango_write_attribute2(devRobotTransY,'position',targetPositionTS);
pause(0.2)

% Start TZ movement
tango_write_attribute2(devRobotTransZ,'position',targetPositionZ);
pause(0.2)


% Read current positions for rotations
    tango_write_attribute2(devRobotRS1,'position',targetRS1);
pause(0.2)
    tango_write_attribute2(devRobotRS2,'position',targetRS2);
pause(0.2)
    tango_write_attribute2(devRobotRS3,'position',targetRS3);
pause(0.2)
    tango_write_attribute2(devRobotRS4,'position',targetRS4);
pause(0.2)

disp('Translation TX TS TZ: all commands sent...')

wait_for_standby_SZR(1,1,1)

disp('Translation TX TS TZ: done.')

end % function
