//Authors:Mohit Chhabra,Adeesh Kale
//January 2016


//Create a handle for the camera
n=camopen()
//Decide the thresholds. The thresolds will change depend upon lighting conditions
//and may be found by hit and trial. If wrong, the program may result in segmentation
//fault due to the SearchBlobs function used later.
//For green object:

lthresh_g_g=144;
lthresh_g_r=43;
lthresh_g_b=142;
Uthresh_g_g=197;
Uthresh_g_r=92;
Uthresh_g_b=203;
//For red object:
lthresh_r_g=124;
lthresh_r_r=220;
lthresh_r_b=115;
Uthresh_r_g=141;
Uthresh_r_r=242;
Uthresh_r_b=141;
//thresh_Area=300;
//thresh_Grab=99; 

try
    //Record the frame
    im=avireadframe(n);
    
    r=im(:,:,1);
    g=im(:,:,2);
    b=im(:,:,3);
    
    //Seperate out the red,green and blue components of the image.
    justSelection_g=g;
    justSelection_b=b;
    justSelection_r=r;
    
    //Try different techniques to obtain the green object.
    black_white_g=(justSelection_g > lthresh_g_g) & (justSelection_g < Uthresh_g_g) & (justSelection_r > lthresh_g_r) & (justSelection_r < Uthresh_g_r) & (justSelection_b > lthresh_g_b) & (justSelection_b < Uthresh_g_b);
        black_white_r=(justSelection_g > lthresh_r_g) & (justSelection_g < Uthresh_r_g)& (justSelection_r > lthresh_r_r) & (justSelection_r < Uthresh_r_r) & (justSelection_b > lthresh_r_b) & (justSelection_b < Uthresh_r_b);
        
        disp('asd');
//        ImageTool(im);
//imshow(black_white_g);
    //blob_g_area=SearchBlobs(black_white_g);
    //filtered_g=FilterBySize(blob_g_area,thresh_Area);
    imshow(black_white_r);
    imshow(black_white_g);
    //Filter out the green component by color and blob size. Or use other algorithms to
    //segment the desired objects
    //Use morphological operations to enhance the image and extract the centroid.
    //Try playing with these functions to get best results.
    gg=black_white_g;   
    
    //gg=filtered_g>0;
//    StructureElement=CreateStructureElement('square',20)
//    gg=CloseImage(gg,StructureElement);
    se = CreateStructureElement('horizontal_line',10);
    gg = ErodeImage(gg, se);
    se = CreateStructureElement('vertical_line',10);
    gg = ErodeImage(gg, se);
    
    
    gg=gg.*1;
    Feat=CreateFeatureStruct(%f);
    Feat.Centroid =%t;
    IsCalculated_g=AnalyzeBlobs(gg,Feat);
    
    //Display the centroid of the target object segmented.
    //You may use try to catch exceptions if errors if the IsCalculated Structure is 
    //empty
    x_g=IsCalculated_g(1).Centroid(1)
    y_g=IsCalculated_g(1).Centroid(2)
    
    //SIMILARLY FOR RED OBJECT
    rr=black_white_r;   
    //gg=filtered_g>0;
//    StructureElement=CreateStructureElement('square',20)
//    rr=CloseImage(rr,StructureElement);
    se = CreateStructureElement('horizontal_line',10);
    rr = ErodeImage(rr, se);
    se = CreateStructureElement('vertical_line',10);
    rr = ErodeImage(rr, se);
    
    
    rr=rr.*1;
    Feat=CreateFeatureStruct(%f);
    Feat.Centroid =%t;
    IsCalculated_r=AnalyzeBlobs(rr,Feat);
    //Display the centroid of the target object segmented.
    //You may use try to catch exceptions if errors if the IsCalculated Structure is 
    //empty
    x_r=IsCalculated_r(1).Centroid(1)
    y_r=IsCalculated_r(1).Centroid(2)
    //Calculate distance between centres
    dist = sqrt((x_r-x_g)^2 + (y_r-y_g)^2);
    //Rest of your logic for giving control instructions to Arduino goes here.
    //Most likey port to be assigned to arduino is ttyACM0. You may need to use
    //sudo chmod 666 /dev/ttyACM0
    //Keep buffer size in mind while writing code.
    //Verify serial communication by echoing. You may use isavialable function.
    avicloseall();
catch
    disp('Seems to be an error somewhere');
    avicloseall();
    closeserial();
