using namespace cv;
using namespace std;
//initial min and max HSV filter values.
//these will be changed using trackbars
int H_MIN = 0;
int H_MAX = 256;
int S_MIN = 0;
int S_MAX = 256;
int V_MIN = 0;
int V_MAX = 256;

const int FRAME_WIDTH = 640; //default capture width and height
const int FRAME_HEIGHT = 480;

const int MAX_NUM_OBJECTS = 50; //max number of objects to be detected in frame

const int MIN_OBJECT_AREA = 20 * 20; //minimum object area
const int MAX_OBJECT_AREA = FRAME_HEIGHT*FRAME_WIDTH / 1.5; //maxium object area

//names that will appear at the top of each window
const string windowName = "Original Image";
const string windowName1 = "HSV Image";
const string windowName2 = "Thresholded Image";
const string windowName3 = "After Morphological Operations";
const string trackbarWindowName = "Trackbars";
void on_trackbar(int, void*)
{//This function gets called whenever a trackbar position is changed
    }
string intToString(int number){


std::stringstream ss;
ss << number;
return ss.str();
    }
void createTrackbars(){
//create window for trackbars

namedWindow(trackbarWindowName, 0);
//create memory to store trackbar name on window
char TrackbarName[50];
sprintf(TrackbarName, "H_MIN", H_MIN);
sprintf(TrackbarName, "H_MAX", H_MAX);
sprintf(TrackbarName, "S_MIN", S_MIN);
sprintf(TrackbarName, "S_MAX", S_MAX);
sprintf(TrackbarName, "V_MIN", V_MIN);
sprintf(TrackbarName, "V_MAX", V_MAX);
//create trackbars and insert them into window
//3 parameters are: the address of the variable that is changing when the trackbar is moved(eg.H_LOW),
//the max value the trackbar can move (eg. H_HIGH),
//and the function that is called whenever the trackbar is moved(eg. on_trackbar)
//                                  ---->    ---->     ---->
createTrackbar("H_MIN", trackbarWindowName, &H_MIN, H_MAX, on_trackbar);
createTrackbar("H_MAX", trackbarWindowName, &H_MAX, H_MAX, on_trackbar);
createTrackbar("S_MIN", trackbarWindowName, &S_MIN, S_MAX, on_trackbar);
createTrackbar("S_MAX", trackbarWindowName, &S_MAX, S_MAX, on_trackbar);
createTrackbar("V_MIN", trackbarWindowName, &V_MIN, V_MAX, on_trackbar);
createTrackbar("V_MAX", trackbarWindowName, &V_MAX, V_MAX, on_trackbar);


}
void drawObject(int x, int y, Mat &frame){

circle(frame, Point(x, y), 20, Scalar(0, 255, 0), 2); //draws circle on object being tracked
circle(frame, Point(x, y), 20, Scalar(0, 0, 255), 2);

putText(frame, intToString(x) + "," + intToString(y), Point(x, y + 30), 1, 1, Scalar(0, 255, 0), 2); //displays coords of object detected next to the object

}

void drawObject2(int a, int b, Mat &frame){

circle(frame, Point(a, b), 20, Scalar(0, 255, 0), 2); //circle around second object
circle(frame, Point(a, b), 20, Scalar(255, 0, 0), 2);

putText(frame, intToString(a) + "," + intToString(b), Point(a, b + 30), 1, 1, Scalar(255, 0, 0), 2);
}

void morphOps(Mat &thresh){

//create structuring element that will be used to "dilate" and "erode" image.
//the element chosen here is a 3px by 3px rectangle

Mat erodeElement = getStructuringElement(MORPH_RECT, Size(3, 3));

//dilate with larger element so make sure object is nicely visible
Mat dilateElement = getStructuringElement(MORPH_RECT, Size(8, 8));

erode(thresh, thresh, erodeElement);
erode(thresh, thresh, erodeElement);


dilate(thresh, thresh, dilateElement);
dilate(thresh, thresh, dilateElement);

}
void trackFilteredObject(int &x, int &y, Mat threshold, Mat &cameraFeed){

Mat temp;
threshold.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function
findContours(temp, contours, hierarchy, CV_RETR_CCOMP,    CV_CHAIN_APPROX_SIMPLE);
//use moments method to find our filtered object
double refArea = 0;
bool objectFound = false;
if (hierarchy.size() > 0) {
    int numObjects = hierarchy.size();
    //if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter
    if (numObjects<MAX_NUM_OBJECTS){
        for (int index = 0; index >= 0; index = hierarchy[index][0]) {

            Moments moment = moments((cv::Mat)contours[index]);
            double area = moment.m00;

            //if the area is less than 20 px by 20px then it is probably just noise
            //if the area is the same as the 3/2 of the image size, probably just a bad filter
            //we only want the object with the largest area so we safe a reference area each
            //iteration and compare it to the area in the next iteration.
            if (area>MIN_OBJECT_AREA && area<MAX_OBJECT_AREA && area>refArea){
                x = moment.m10 / area;
                y = moment.m01 / area;
                objectFound = true;
                refArea = area;
            }
            else objectFound = false;

        }
        //let user know you found an object
        if (objectFound == true){
            drawObject(x, y, cameraFeed);
        }
    }
}
}


void trackFilteredObject2(int &a, int &b, Mat threshold, Mat &cameraFeed){

Mat temp;
threshold.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function
findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
//use moments method to find our filtered object
double refArea = 0;
bool objectFound2 = false;
if (hierarchy.size() > 0) {
    int numObjects = hierarchy.size();
    //if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter
    if (numObjects < MAX_NUM_OBJECTS){
        for (int index = 0; index >= 0; index = hierarchy[index][0]) {

            Moments moment = moments((cv::Mat)contours[index]);
            double area = moment.m00;

            //if the area is less than 20 px by 20px then it is probably just noise
            //if the area is the same as the 3/2 of the image size, probably just a bad filter
            //we only want the object with the largest area so we safe a reference area each
            //iteration and compare it to the area in the next iteration.
            if (area > MIN_OBJECT_AREA && area<MAX_OBJECT_AREA && area>refArea){
                a = moment.m10 / area;
                b = moment.m01 / area;
                objectFound2 = true;
                refArea = area;
            }
            else objectFound2 = false;
        }
        //let user know you found an object
        if (objectFound2 == true){
            drawObject2(a, b, cameraFeed);
        }
    }
}
}



int main(int argc, char* argv[])
{
//some boolean variables for different functionality within this
//program

bool trackObjects = true;
bool useMorphOps = true;


Mat cameraFeed; //Matrix to store each frame of the webcam feed

Mat HSV; //matrix storage for HSV image

Mat threshold; //matrix storage for binary threshold image


//x and y values for the location of the object
int x = 0, y = 0;
int a = 0, b = 0;

createTrackbars(); //create slider bars for HSV filtering

VideoCapture capture; //video capture object to acquire webcam feed

capture.open(0); //opens webcam feed

capture.set(CV_CAP_PROP_FRAME_WIDTH, FRAME_WIDTH); //set height of capture frame
capture.set(CV_CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT); //set width of capture fram

//start an infinite loop where webcam feed is copied to cameraFeed matrix
while (true)
{

    capture.read(cameraFeed); //store image to matrix

    cvtColor(cameraFeed, HSV, COLOR_BGR2HSV); //convert frame from BGR to HSV colorspace

    //filter HSV image between values and store filtered image to
    //threshold matrix
    inRange(HSV, Scalar(H_MIN, S_MIN, V_MIN), Scalar(H_MAX, S_MAX, V_MAX), threshold);

    //perform morphological operations on thresholded image to eliminate noise
    //and emphasize the filtered object(s)
    if (useMorphOps)
        morphOps(threshold);
    //pass in thresholded frame to our object tracking function
    //this function will return the x and y coordinates of the
    //filtered object
    if (trackObjects)
        trackFilteredObject(x, y, threshold, cameraFeed);

    if (trackObjects)
        trackFilteredObject2(a, b, threshold, cameraFeed);


    //show frames
    imshow(windowName2, threshold);
    imshow(windowName, cameraFeed);


    //delay 30ms so that screen can refresh.
    //image will not appear without this waitKey() command
    if (waitKey(1) == 27)break;
}


return 0;
}