%% init the controllers
f1 = figure();
f2 = figure();
tstage = actxcontrol('MGMOTOR.MGMotorCtrl.1', [20 20 600 400], f1);
rstage = actxcontrol('MGMOTOR.MGMotorCtrl.1', [20 20 600 400], f2);
set(tstage, 'HWSerialNum', 27001029);
set(rstage, 'HWSerialNum', 27501183);
tstage.StartCtrl();
rstage.StartCtrl();

%% set initial positions and references
fprintf(2, 'Moving stages to initial position...\n')

% move to home position
tstage.MoveHome(0, true);
rstage.MoveHome(0, true);

% move tstage by 25 mm
tstage.SetRelMoveDist(0,25.00);
tstage.SetVelParams(0, 2, 2, 2);
tstage.MoveRelative(0, true);

% hack to get the notification of when the motion is over
global moving;
tstage.registerevent({'MoveComplete', 'move_complete'});
rstage.registerevent({'MoveComplete', 'move_complete'});

% here we should adjust the rotation stage so that it is porperly aligned
% in the reference starting position

%% polishing steps

% laser position
a0 = acos(36/(75/2));  
laser = [-75/2*sin(a0) + 25, 75/2*cos(a0)];

figure();
draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
fprintf(2, 'Set wafer in reference position before moving forward...\n');
pause

tstage_vel = 1;
rstage_vel = 4;
tstage_dist = 22.00;
rstage_rot = 180;

% translation - 22 mm
fprintf(2, 'Translation -%d mm...\n', tstage_dist);
tstage.SetRelMoveDist(0, -tstage_dist);
tstage.SetVelParams(0, tstage_vel, tstage_vel, tstage_vel);
tstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser);
    drawnow;
    pause(0.1);
end
   
% rotation 180 degrees CCW (negative direction)
fprintf(2, 'Rotation %d degrees CCW...\n', rstage_rot);
rstage.SetRelMoveDist(0, -rstage_rot);
rstage.SetVelParams(0, rstage_vel, rstage_vel, rstage_vel);
rstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
    drawnow
    pause(0.1);
end

% translation + 22 mm
fprintf(2, 'Translation +%d mm...\n', tstage_dist);
tstage.SetRelMoveDist(0, tstage_dist);
tstage.SetVelParams(0, tstage_vel, tstage_vel, tstage_vel);
tstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
    drawnow;
    pause(0.1);
end

% translation - 22 mm
fprintf(2, 'Translation -%d mm...\n', tstage_dist);
tstage.SetRelMoveDist(0, -tstage_dist);
tstage.SetVelParams(0, tstage_vel, tstage_vel, tstage_vel);
tstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
    drawnow;
    pause(0.1);
end

% rotation 180 degrees CCW (negative direction)
fprintf(2, 'Rotation %d degrees CCW...\n', rstage_rot);
rstage.SetRelMoveDist(0, -rstage_rot);
rstage.SetVelParams(0, rstage_vel, rstage_vel, rstage_vel);
rstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
    drawnow;
    pause(0.1);
end

% translation + 21 mm
fprintf(2, 'Translation +%d mm...\n', tstage_dist);
tstage.SetRelMoveDist(0, tstage_dist);
tstage.SetVelParams(0, tstage_vel, tstage_vel, tstage_vel);
tstage.MoveRelative(0, false);
pause(0.1);
moving = true;
while moving
    draw_wafer(tstage.GetPosition_Position(0), 90+rstage.GetPosition_Position(0), laser)
    drawnow;
    pause(0.1);
end


%% close connection
tstage.StopCtrl();
rstage.StopCtrl();