clear;clc;
disp('Scan Inializing........')
tic;
t1 = toc;
s = serial('COM3');
set(s,'BaudRate',57600);   set(s,'Terminator','#');    
set(s,'Timeout',30);    %s.RecordName = 'CytoSerialTxnLog.txt';    
fopen(s);  %recweblistord(s);
msg=strcat(datestr(clock,'yyyy-mm-dd-HHMM'),'m',datestr(clock,'ss'),'s'); 
fid = fopen(strcat(msg,'.txt'),'w');
if (fid == -1)
    disp('could not open logging file. You probably need to change working directory to the one where the MATLAB file is located.');
    return;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Start cam
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
disp('\r\nStarting camera...')
fprintf(fid, '\r\nStarting camera...');
% choose which webcam (winvideo-1) and which  mode (YUY2_176x144)
%vid = videoinput('winvideo', 1, 'RGB32_744x480');
vid = videoinput('gentl', 1, 'BGRA8Packed');
% Configure the object for manual trigger mode.
triggerconfig(vid, 'manual');
% only capture one frame per trigger, we are not recording a video
vid.FramesPerTrigger = 1;
% output would image in RGB color space
vid.ReturnedColorspace = 'rgb';
% tell matlab to start the webcam on user request, not automatically
triggerconfig(vid, 'manual');
% we need this to know the image height and width
vidRes = get(vid, 'VideoResolution');
% image width
imWidth = vidRes(1);
% image height
imHeight = vidRes(2);
% number of bands of our image (should be 3 because it's RGB)
nBands = get(vid, 'NumberOfBands');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do auto white balance
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
src = getselectedsource(vid);
src.BalanceWhiteAuto = 'Continuous'; 

src.ExposureTime  = 800;
start(vid);
Time_to_init_cam = toc

% this is the slide name 
setslidename = 'Slide';


fprintf(s,'J');currxpos = 0;
fprintf(s,'K');currypos = 0;
fprintf(s,'L');currzpos = 0;
pause(0.1);
disp('Origin is set');

xFoV = round(1280*5.3/(34*0.159));yFoV = round(1024*5.3/(34*0.159));
xFoV_mm = 0.1995;yFoV_mm = 0.1596;

XPulseRate = 38400;     YPulseRate = 38400 ; 
Xzadjustdelay = 0.002;
autoContrTime = 0.1;
XImDelay = (xFoV / XPulseRate) ;
YImDelay = (yFoV / YPulseRate) ;
%at y = 12.5mm line
Ypos = strcat('+',sprintf('%06d',abs(41359)));
fprintf(s,'N'); fprintf(s,Ypos);pause(41359/YPulseRate);currypos=41359;
param_y0 =[];Zval=[];
xpos1 = 0; xpos2 = 200000;
[currzpos, temp_var] = focusstack_1(s,vid,currzpos);pause(2)
tf1 = toc;
for xpos = xpos1:17986:xpos2
    Xpos = strcat('+',sprintf('%06d',abs(xpos )));
    fprintf(s,'B'); fprintf(s,Xpos);pause(17986/XPulseRate)
    [currzpos, temp_var] = focusstack_2(s,vid,currzpos);
    Zval = [Zval currzpos]; 
    snapshot=getsnapshot(vid);
    disp('saving best focussed snapshot..')
    imwrite(snapshot,strcat('images/finding_best_region/t7/t7_x_',Xpos,'_',sprintf('%06d',abs(currzpos)),'.bmp'));
    param_y0 = [param_y0 parameter(snapshot)];    currxpos = xpos;
end
disp(toc-tf1)
xpos = xpos1:17986:xpos2;
[max_param_y0, Index] = max(param_y0);
xbest1 = abs(xpos(Index));
Xbest1 = strcat('+',sprintf('%06d',xbest1));
fprintf(s,'B'); fprintf(s,Xpos);pause(abs(currxpos-xbest1)/XPulseRate)
Zfoc1 = Zval(Index);gotoZ(s,Zfoc1,currzpos);currzpos=Zfoc1;param_y1 = []; Zval=[];
tf2 =toc;
if(xbest1 <= 17986)
    for xpos = (xbest1):7194:(xbest1+17986*2)
        Xpos = strcat('+',sprintf('%06d',abs(xpos )));
        fprintf(s,'B'); fprintf(s,Xpos);pause(17986/XPulseRate)
        [currzpos, temp_var] = focusstack_2(s,vid,currzpos);
        Zval = [Zval currzpos]; 
        snapshot=getsnapshot(vid);
        disp('saving best focussed snapshot..')
        imwrite(snapshot,strcat('images/finding_best_region/t7/t7_x_',Xpos,'_',sprintf('%06d',abs(currzpos)),'.bmp'));
        param_y1 = [param_y1 parameter(snapshot)];    currxpos = xpos;
    end
elseif(xbest1>=200000)
    for xpos = (xbest1-17986*2):7194:(xbest1)
        Xpos = strcat('+',sprintf('%06d',abs(xpos )));
        fprintf(s,'B'); fprintf(s,Xpos);pause(17986/XPulseRate)
        [currzpos, temp_var] = focusstack_2(s,vid,currzpos);
        Zval = [Zval currzpos]; 
        snapshot=getsnapshot(vid);
        disp('saving best focussed snapshot..')
        imwrite(snapshot,strcat('images/finding_best_region/t7/t7_x_',Xpos,'_',sprintf('%06d',abs(currzpos)),'.bmp'));
        param_y1 = [param_y1 parameter(snapshot)];    currxpos = xpos;
    end
else
    for xpos = (xbest1-17986):7194:(xbest1+17986)
        Xpos = strcat('+',sprintf('%06d',abs(xpos )));
        fprintf(s,'B'); fprintf(s,Xpos);pause(17986/XPulseRate)
        [currzpos, temp_var] = focusstack_2(s,vid,currzpos);
        Zval = [Zval currzpos]; 
        snapshot=getsnapshot(vid);
        disp('saving best focussed snapshot..')
        imwrite(snapshot,strcat('images/finding_best_region/t7/t7_x_',Xpos,'_',sprintf('%06d',abs(currzpos)),'.bmp'));
        param_y1 = [param_y1 parameter(snapshot)];    currxpos = xpos;
    end
end
disp(toc-tf2)
xpos = (xbest1-17896):7194:(xbest1+17896);
param_y1 = (param_y1>0.0999);disp(param_y1)
target1 = sum(param_y1(1:3)); target2 = sum(param_y1(3:5));
fprintf(s,'N'); fprintf(s,'0000000');pause(abs(41359)/YPulseRate);
tf3=toc;
if((sum(param_y1(2:4))==3))
    xbest2 = xpos(2);xbest1 = xpos(4);
    Xbest2 = strcat('+',sprintf('%06d',xbest2));
    Xbest1 = strcat('+',sprintf('%06d',xbest1));
    fprintf(s,'B'); fprintf(s,Xbest1);pause((abs(currxpos-xbest1))/XPulseRate);gotoZ(s,Zval(4),currzpos);currzpos=Zval(4);fprintf(s,'R');
    fprintf(s,'B'); fprintf(s,Xbest2);pause((abs(17986))/XPulseRate);gotoZ(s,Zval(2),currzpos);currzpos=Zval(2);fprintf(s,'E');currxpos = xbest2;
    fprintf(s,'N'); fprintf(s,'0078719');pause(abs(78719)/YPulseRate);currypos = 78719;
    [currzpos, temp_var] = focusstack_2(s,vid,currzpos);fprintf(s,'Y');
    fprintf(s,'Z');
    xA = xbest2; xB = xbest1;
    yC = 78719; yB =0;
    numofX = (xB - xA) / xFoV;       
    numofY = (yC - yB) / yFoV;
    while fscanf(s) ~= 'S'
    end;
    % NOW TAKING PICTURES
    rowcount = 0;
    filecount = 0;
    
    msg = '\r\n Starting image capture ...';
    fprintf(fid, msg);
    
    % Dircn = false; %We believe we start from top
    while(rowcount <= numofY)
        while(filecount <= numofX)
            %pause(XImDelay*2);
            snapshot=getsnapshot(vid);
            %enh_snapshot=autocontrast(snapshot);
            filename = strcat(setslidename,'_',num2str(rowcount),'-',num2str(filecount), '.bmp');
            %imwrite(enh_snapshot,filename);
            imwrite(snapshot,strcat('images/',filename));
            filecount = filecount + 1;
        end
        % wait for shift by one FoV in Y direction
        filecount = 0;
        %pause(YImDelay*2);
        rowcount = rowcount + 1;
    end
elseif(target1>target2)
    xbest2 = xpos(1);xbest1 = xpos(3);
    Xbest2 = strcat('+',sprintf('%06d',xbest2));
    Xbest1 = strcat('+',sprintf('%06d',xbest1));
    fprintf(s,'B'); fprintf(s,Xbest1);pause((abs(currxpos-xbest1))/XPulseRate);gotoZ(s,Zval(3),currzpos);currzpos=Zval(3);fprintf(s,'R');
    fprintf(s,'B'); fprintf(s,Xbest2);pause((abs(17986))/XPulseRate);gotoZ(s,Zval(1),currzpos);currzpos=Zval(1);fprintf(s,'E');currxpos = xbest2;
    fprintf(s,'N'); fprintf(s,'0078719');pause(abs(78719)/YPulseRate);currypos = 78719;
    [currzpos, temp_var] = focusstack_2(s,vid,currzpos);fprintf(s,'Y');
    fprintf(s,'Z');
    xA = xbest2; xB = xbest1;
    yC = 78719; yB =0;
    numofX = (xB - xA) / xFoV;       
    numofY = (yC - yB) / yFoV;
    while fscanf(s) ~= 'S'
    end;
    % NOW TAKING PICTURES
    rowcount = 0;
    filecount = 0;
    
    msg = '\r\n Starting image capture ...';
    fprintf(fid, msg);
    
    % Dircn = false; %We believe we start from top
    while(rowcount <= numofY)
        while(filecount <= numofX)
            %pause(XImDelay*2);
            snapshot=getsnapshot(vid);
            %enh_snapshot=autocontrast(snapshot);
            filename = strcat(setslidename,'_',num2str(rowcount),'-',num2str(filecount), '.bmp');
            %imwrite(enh_snapshot,filename);
            imwrite(snapshot,strcat('images/',filename));
            filecount = filecount + 1;
        end
        % wait for shift by one FoV in Y direction
        filecount = 0;
        %pause(YImDelay*2);
        rowcount = rowcount + 1;
    end
elseif(target1<target2)
    xbest2 = xpos(5);xbest1 = xpos(3);
    Xbest2 = strcat('+',sprintf('%06d',xbest2));
    Xbest1 = strcat('+',sprintf('%06d',xbest1));
    fprintf(s,'B'); fprintf(s,Xbest1);pause((abs(currxpos-xbest1))/XPulseRate);gotoZ(s,Zval(3),currzpos);currzpos=Zval(3);fprintf(s,'E');
    fprintf(s,'B'); fprintf(s,Xbest2);pause((abs(17986))/XPulseRate);gotoZ(s,Zval(5),currzpos);currzpos=Zval(5);fprintf(s,'R');currxpos = xbest2;
    fprintf(s,'N'); fprintf(s,'0078719');pause(abs(78719)/YPulseRate);currypos = 78719;
    [currzpos, temp_var] = focusstack_2(s,vid,currzpos);fprintf(s,'Y');
    fprintf(s,'Z');
    xA = xbest1; xB = xbest2;
    yC = 78719; yB =0;
    numofX = (xB - xA) / xFoV;       
    numofY = (yC - yB) / yFoV;
    while fscanf(s) ~= 'S'
    end;
    % NOW TAKING PICTURES
    rowcount = 0;
    filecount = 0;
    
    msg = '\r\n Starting image capture ...';
    fprintf(fid, msg);
    
    % Dircn = false; %We believe we start from top
    while(rowcount <= numofY)
        while(filecount <= numofX)
            %pause(XImDelay*2);
            snapshot=getsnapshot(vid);
            %enh_snapshot=autocontrast(snapshot);
            filename = strcat(setslidename,'_',num2str(rowcount),'-',num2str(filecount), '.bmp');
            %imwrite(enh_snapshot,filename);
            imwrite(snapshot,strcat('images/t7/',filename));
            filecount = filecount + 1;
        end
        % wait for shift by one FoV in Y direction
        filecount = 0;
        %pause(YImDelay*2);
        rowcount = rowcount + 1;
    end
else    
    disp('Slide is not prepared well!!! Please make a good slide..!')
end
disp(toc-tf3)
    
stop(vid);
fprintf(s,'N'); fprintf(s,'0000000');
fprintf(s,'B'); fprintf(s,'0000000');
fprintf(s,'M'); fprintf(s,'0000000');
pause(0.1)

if ~isempty(instrfind)
    fclose(instrfind);
    delete(instrfind);
end
t2 = toc;
disp((t2-t1)/60)