%Scott Mueller
%3/10/09
%main.m
%Description:
%Main Processing Loop For Vision Controller Nerf Gun
%Requires Matlab Image Acquisition Toolbox, Webcam, 2 USB-TTL cables and
%PIC MASTER/SLAVE Servo Drive with Green Laser Pointer
%Targets Must be light colored against a black background

vidobj = videoinput('winvideo');  %create the video object

%pwmToPixel = servoCalibration();  %performs the servo calibration routine

Im = getsnapshot(vidobj); %get an image
Im = ImApplyROI(Im); %apply the region of interest box:  must draw box on displayed image with mouse
%this ROI must lay entirely within the black background

%//////////////////////CREATING RGB TARGET IMAGE///////////////////////////

ImTargetListRGB = Im .* 0;
%//////////////////////////////////////////////////////////////////////////


%imshow(snapshot);
imagesc(Im);  %displays image in frame with pixel tools

%///////////////////////GENERIC IMAGE PROCESSING///////////////////////////
level = graythresh(Im);  % mean brightness level from histogram
%ImB = Im2bw(Im,level);  %create binary image based on threshold
ImB = Im(:,:,1) > 100;
%ImB = double(ImB) + double(Im(:,:,3) > 80);

pixelThresh = 150; %min size of objects
ImB = bwareaopen(ImB,pixelThresh);  %remove objects  < pixelThresh
ImB= imfill(ImB, 'holes'); %fill in holes in objects
ImL = bwlabel(ImB);  %create label matrix from binary 
imshow(ImL) %display the labe
pause(10);
%//////////////////////////////////////////////////////////////////////////

%///////creating appropriate parameters to initialize target list//////////
color = ones(3,1); %rgb color vector
area = 98; %scalar area of target
centroid = ones(1,2); %[x;y] coordinates of centroid  *not pixel coords!
perimeter = 19; %scalar perimeter of target
shape = 3;%shape index: references list in getShape.m
binMask = ImB .* 0; %binary image containing only the respective target
%//////////////////////////////////////////////////////////////////////////

targetListStats = regionprops(ImL,'area','centroid','perimeter'); %creates a list of regions' areas
newTarget = struct('color',color,'area', area, 'centroid',centroid, 'perimeter',perimeter, 'binMask', binMask,'shape',shape); %target object
TargetList = struct('color',color,'area', area, 'centroid',centroid, 'perimeter',perimeter, 'binMask', binMask,'shape',shape); %list of target objects

targetCentroids = regionprops (ImL,'Centroid'); %uses builtin function to get a [m,2] vector of object centroids in ImL, the label maxtrix image

temp = ones(1,2);  %used for getting length for loop
temp = size(targetListStats); %used for getting length for loop
listSize = temp(1,1);  %used for getting length for loop
%targetListStats



%/////////creates first target object and adds it to the list//////////////
    newTarget.area = targetListStats(1).Area;
    newTarget.centroid = targetListStats(1).Centroid;
    newTarget.perimeter = targetListStats(1).Perimeter;
    newTarget.binMask = zeros(size(ImB))+(ImL == 1);
    newTarget.color = Im(round(newTarget.centroid(1,2)),round(newTarget.centroid(1,1)),:);
    newTarget.shape = getShape(newTarget.perimeter, newTarget.area);
    TargetList = newTarget;  % add target to list
%//////////////////////////////////////////////////////////////////////////


%creates an RGB image containing only the targets
%this is accomplished by multiplying the binary mask of each target by the
%original image Im for each component R,G,B and adding the respective
%product to the respective component of ImRGB
    for i=1:3 %loop through RGB indices of image
        ImTargetListRGB(:,:,i) = double(ImTargetListRGB(:,:,i)) + double(newTarget.binMask) .* double(newTarget.color(i));   
        %imshow(ImTargetListRGB);
    end



    for j=2:listSize  %loop through all found targets by using areas vector 
        
        newTarget.area = targetListStats(j).Area; 
        newTarget.centroid = targetListStats(j).Centroid;
        newTarget.perimeter = targetListStats(j).Perimeter;
        newTarget.binMask = zeros(size(ImB))+(ImL == j);
        newTarget.shape = getShape(newTarget.perimeter,newTarget.area);
        
        %the color of the target is whatever the color of its centroid is in the original image Im
        newTarget.color = Im(round(newTarget.centroid(1,2)),round(newTarget.centroid(1,1)),:);  
        
        tempIm = Im .*0;
        tempIm(:,:,1)= newTarget.color(1);
        tempIm(:,:,2)= newTarget.color(2);
        tempIm(:,:,3)= newTarget.color(3);
        %imshow(tempIm);
       % pause(10);
        %newTarget
        TargetList = [TargetList ; newTarget];  % add target to list

        for i=1:3  %add target to ImTargetListRGB
            newTarget.color;
            tmpcolor = double(getColorCentroid(Im,TargetList(j)));
            ImTargetListRGB(:,:,i) = double(ImTargetListRGB(:,:,i)) + double(newTarget.binMask) .* double(tmpcolor(i)); 
        end
        imshow(ImTargetListRGB); %display
    end



%//////DISPLAYS TARGETS WITH CENTROID AND SHAPE LABELS IN COLOR////////////
hold on
imshow(ImTargetListRGB);
tempSize=size(TargetList) %gets dimensions of the targetlist
for i=1:tempSize(1); %loop through target list
    r = TargetList(i).centroid(1,1);
    c = TargetList(i).centroid(1,2);
    plot(r, c, 'b*'),
    %text(305,130.5,TargetList(2).shape,'Color','y','FontSize',14,'FontWeight','bold');
    text(TargetList(i).centroid(1,1)-15,TargetList(i).centroid(1,2)+20,TargetList(i).shape,'Color','b',...
      'FontSize',14,'FontWeight','bold');5
    text(TargetList(i).centroid(1,1)-15,TargetList(i).centroid(1,2)-20,'Target','Color','b',...
      'FontSize',14,'FontWeight','bold');
end
hold off
%//////////////////////////////////////////////////////////////////////////


%//////////////////////////////////////////////////////////////////////////
%SHOOTING TARGETS
%This steps through the list of targets, calculates the servo angles to
%fire at each target's centroid and fires the gun
for i=1:tempSize
    TargetCentroid = TargetList(i).centroid;
    TargetCentroidy = TargetCentroid(1);
    TargetCentroidx = TargetCentroid(2);
    tilt = round(interp1(pwmToPixel(2,:),pwmToPixel(1,:),TargetCentroidy,'linear','extrap')) %tilt is servo position in us pulsewidth
    pan = round(interp1(pwmToPixel(4,:),pwmToPixel(3,:),TargetCentroidx, 'linear','extrap')) %pan is servo position in us pulsewidth
    panTilt(tilt,pan); %send command to master servo controller to move to position
    pause(3);
    %Fire();
    pause(10);
end
%//////////////////////////////////////////////////////////////////////////






