import libjevois as jevois
import cv2
import numpy as np

class DetectGreenObjects:
    def __init__(self):
        self.stuff = 0

    def process(self, inframe, outframe):
        source0 = inimg = inframe.getCvBGR()
        outimg = inimg = inframe.getCvBGR()
        self.timer.start()

    def getArea(con): # Gets the area of the contour
            return cv2.contourArea(con)

    def getYcoord(con): # Gets the Y coordinate of the contour
        M = cv2.moments(con)
        cy = int(M['m01']/M['m00'])
        return cy

    def getXcoord(con):  # Gets the X coordinate of the contour
        M = cv2.moments(con)
        cy = int(M['m10']/M['m00'])
        return cy

    def sortByArea(conts):  # Returns an array sorted by area from smallest to largest
        contourNum = len(conts)  # Gets number of contours
        # sortedBy now has all the contours sorted by area
        sortedBy = sorted(conts, key=getArea)
        return sortedBy

    # Draws all contours on original image in red
    cv2.drawContours(outimg, self.filter_contours_output, -1, (0, 0, 255), 1)

        # Gets number of contours
    contourNum = len(self.filter_contours_output)

        # Sorts contours by the smallest area first
    newContours = sortByArea(self.filter_contours_output)

        # Send the contour data over Serial
    for i in range(contourNum):
        cnt = newContours[i]
            # Get the stats of the contour including width and height
        x, y, w, h = cv2.boundingRect(cnt)

            # which contour, 0 is first
        toSend = ("CON" + str(i) +
                      "area" + str(getArea(cnt)) +  # Area of contour
                      # x-coordinate of contour, -500 to 500 rounded to 2 decimal
                      "x" + str(round((getXcoord(cnt)*1000/320)-500, 2)) +
                      # y-coordinate of contour, -375 to 375 rounded to 2 decimal
                      "y" + str(round(375-getYcoord(cnt)*750/240, 2)) +
                      # Height of contour, 0-750 rounded to 2 decimal
                      "h" + str(round(h*750/240, 2)) +
                      "w" + str(round(w*1000/320, 2)))  # Width of contour, 0-1000 rounded to 2 decimal
        jevois.sendSerial(toSend)

        # Write a title:
    cv2.putText(outimg, "Aaron's JeVois Code", (3, 20),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA)

        # Write frames/s info from our timer into the edge map (NOTE: does not account for output conversion time):
    fps = self.timer.stop()
        #height, width, channels = outimg.shape # if outimg is grayscale, change to: height, width = outimg.shape
    height, width, channels = outimg.shape
    cv2.putText(outimg, fps, (3, height - 6),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA)

        # Convert our BGR output image to video output format and send to host over USB. If your output image is not
        # BGR, you can use sendCvGRAY(), sendCvRGB(), or sendCvRGBA() as appropriate:
    outframe.sendCvBGR(outimg)
        # outframe.sendCvGRAY(outimg)