#! /usr/bin/env python
"""
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
"""
print "OpenCV Python motion contours test"

import sys,math

# import the necessary things for OpenCV
from opencv import cv
from opencv import highgui

#############################################################################
# so, here is the main part of the program

if __name__ == '__main__':

    # first, create the necessary windows
    highgui.cvNamedWindow ('Camera', highgui.CV_WINDOW_AUTOSIZE)
    highgui.cvNamedWindow ('Contours', highgui.CV_WINDOW_AUTOSIZE)

    # move the new window to a better place
    highgui.cvMoveWindow ('Camera', 10, 10)
    highgui.cvMoveWindow ('Contours', 10, 270)

    if len (sys.argv) == 1:
        # no argument on the command line, try to use the camera
        capture = highgui.cvCaptureFromCAM (1)

        #print highgui.cvGetCaptureProperty (capture,
        #                              highgui.CV_CAP_PROP_FPS),
        print highgui.cvGetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_WIDTH),
        print highgui.cvGetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_HEIGHT)
        # set the wanted image size from the camera
        highgui.cvSetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FPS, 30)
        highgui.cvSetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_WIDTH, 640)
        highgui.cvSetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_HEIGHT,480)
        highgui.cvSetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_WIDTH, 320)
        highgui.cvSetCaptureProperty (capture,
                                      highgui.CV_CAP_PROP_FRAME_HEIGHT,240)
    else:
        # we have an argument on the command line,
        # we can assume this is a file name, so open it
        capture = highgui.cvCaptureFromFile(sys.argv [1])            

    # check that capture device is OK
    if not capture:
        print "Error opening capture device"
        sys.exit (1)
        
    # capture the 1st frame to get some propertie on it
    frame = highgui.cvQueryFrame (capture)

    # get some properties of the frame
    frame_size = cv.cvGetSize (frame)

    frame_thresh = cv.cvCreateImage(frame_size, 8, 1)

    frame_contours = cv.cvCreateImage (frame_size, 8, 3)

    while 1:
        # do forever

        # 1. capture the current image
        frame = highgui.cvQueryFrame (capture)
        if frame is None:
            # no image captured... end the processing
            break

        # mirror the captured image
        cv.cvFlip (frame, None, 1)

        # compute the hsv version of the image 
        cv.cvCvtColor(frame,frame_thresh, cv.CV_BGR2GRAY)
 
        cv.cvSmooth(frame_thresh,frame_thresh,cv.CV_GAUSSIAN,3,0,0)

        cv.cvThreshold(frame_thresh,frame_thresh,200,255,cv.CV_THRESH_BINARY)
 
        nb_contours, contours = cv.cvFindContours (frame_thresh, cv.sizeof_CvContour,
                                               cv.CV_RETR_EXTERNAL,
                                               cv.CV_CHAIN_APPROX_SIMPLE,
                                               cv.cvPoint (0,0))

        # comment this out if you do not want approximation
        cv.cvSetZero (frame_contours)
        if contours:
            contours = cv.cvApproxPoly (contours, cv.sizeof_CvContour,
                                    cv.CV_POLY_APPROX_DP, 3, 1)
            #moments = cv.CvMoments()
            contour = contours
            while contour:
                moments = cv.CvMoments()
                cv.cvMoments(contour,moments,1)
                if moments.m00 != 0:
                    area = moments.m00
                    x = moments.m10/moments.m00
                    y = moments.m01/moments.m00
                    #cv.cvCircle(frame_contours,point,int(math.sqrt(moments.m00)),cv.cvScalar(0,255,0,0),-1,8,0)
                    u20 = moments.m20/moments.m00 - x*x
                    u02 = moments.m02/moments.m00 - y*y
                    u11 = moments.m11/moments.m00 - x*y
                    rot = False
                    if rot:
	                    if u20 != u02:
	                        angle = -1 * int(math.atan(2*u11/(u20-u02)) * 45.0)
	                    paxis1 = math.sqrt((u20+u02)/2.0 + math.sqrt(4.0*(u11**2) + (u20 - u02)**2)/2.0)
	                    paxis2 = math.sqrt((u20+u02)/2.0 - math.sqrt(4.0*(u11**2) + (u20 - u02)**2)/2.0)
	                    #axis2 = (moments.m20/moments.m00+moments.m02/moments.m00)/2.0 - math.sqrt(4.0*((moments.m11/moments.m00)**2) +  (moments.m20/moments.m00-moments.m02/moments.m00)**2)/2.0
	                    #print axis1,axis2
	                    axis1 = paxis1/(paxis1+paxis2) * math.sqrt(area)
	                    axis2 = paxis2/(paxis1+paxis2) * math.sqrt(area)
	                    point = cv.cvPoint(int(x),int(y))
	                    cv.cvEllipse(frame_contours,point,
	                               cv.cvSize(int(axis1),int(axis2)),
	                               angle,0,360,
	                               cv.cvScalar(0,255,0,0),-1,8,0)
                    else:
	                    point = cv.cvPoint(int(x),int(y))
	                    cv.cvEllipse(frame_contours,point,
	                               cv.cvSize(int(math.sqrt(area)),int(math.sqrt(area))),
	                               90,0,360,
	                               cv.cvScalar(0,255,0,0),-1,8,0)
                contour = contour.h_next
            
            #cv.cvDrawContours(frame_contours,contours,cv.cvScalar(0,0,255,0),cv.cvScalar(0,255,0,0),3,3, cv.CV_AA,
                           #cv.cvPoint (0, 0))
        
        # compute which pixels are in the wanted range
        # we can now display the images
        highgui.cvShowImage ('Camera', frame)
        highgui.cvShowImage ('Contours', frame_contours)

        # handle events
        k = highgui.cvWaitKey (10)

        if k == 27:
            # user has press the ESC key, so exit
            break