"""
CAMERA CONTROLLER
This script is written to control an OpenMV camera. Its purpose is to instruct the camera to locate the brightest pixels it
can see and send an electric signal through its i/o pins to a raspberry pi until those pixels are no longer in view.
"""
import sensor, time
import pyb

# According to google, VGA resolution is (640width x 480height pixels). These dimensions are used to calculate the field of view of the camera on startup.
VGA_WIDTH = 640 # 640
VGA_HEIGHT = 480 # 480

# TODO: summarize at a high level what each of these functions do
"""FUNCTIONS
configure_sensor -> this function etc....
create_search_rectangle ->

"""
def configure_sensor(pix_format=sensor.GRAYSCALE, framesize=sensor.VGA, skip_frames_time=2000, auto_gain=False, auto_white_balance=False) -> None:
    """This function configures the OpenMV cam sensor to take in images a certain way based on the following arguments.

    Args:
        pix_format (sensor.<FORMAT_TYPE>, optional): Determines the color of the frames taken by the camera. Defaults to sensor.GRAYSCALE.
        framesize (int, optional): Determines the size of each frame taken by the camera. Defaults to sensor.VGA.
        skip_frames_time (int, optional): Not sure what this does lol. Defaults to 2000.
        auto_gain (bool, optional): Determines whether the camera will attempt to automatically adjust itself. Defaults to False.
        auto_white_balance (bool, optional): Determines whether the camera will attempt to balance out images it takes. Defaults to False.
    """
    sensor.reset()
    sensor.set_pixformat(pix_format)
    sensor.set_framesize(framesize)
    sensor.skip_frames(time=skip_frames_time)
    sensor.set_auto_gain(auto_gain) # must be turned off for color tracking
    sensor.set_auto_whitebal(auto_white_balance) # must be turned off for color tracking

def create_search_rectangle(camera_x=0, camera_y=0, box_x=640, box_y=480) -> tuple:
    """This function determines the dimensions of the frames the camera will record.

    Args:
        camera_x (int, optional): IDK. Defaults to 250.
        camera_y (int, optional): IDK. Defaults to 150.
        box_x (int, optional): IDK. Defaults to 150.
        box_y (int, optional): IDK. Defaults to 20.

    Returns:
        tuple<int>: Returns a tuple of ints containing the coordinates of the search box.
    """
    sensor_width = sensor.width()
    sensor_height = sensor.height()

    search_rect_xC = int(round(sensor_width*(camera_x/VGA_WIDTH))) # ?
    search_rect_yC = int(round(sensor_height*(camera_y/VGA_HEIGHT))) # ?
    search_rect_width = int(round(sensor_width*(box_x/VGA_WIDTH))) # ?
    search_rect_height = int(round(sensor_height*(box_y/VGA_HEIGHT))) # ?

    search_rectangle = [search_rect_xC, search_rect_yC, search_rect_width, search_rect_height]

    return search_rectangle

def mark_particles_in_frame(particles: list, image) -> None:
    """This function marks particles in the current frame being observed given the locations of the particles in that frame.

    Args:
        particles (list<image.blob>): A list of blob objects returned by a method provided by the OpenMV sensor class.
            Documented here: https://docs.openmv.io/library/omv.image.html#image.image.Image.image.find_blobs
    """
    rectangle_color = 0 # can be any value from 0 to 255, which correspond to black and white respectively.
    rectangle_side_thickness = 5 # thickness is similar to lined margins Microsoft Word or something
    for particle in particles:
        image.draw_rectangle((particle.rect()), rectangle_color, rectangle_side_thickness)
        image.draw_cross((particle.cx(), particle.cy()), rectangle_color, rectangle_side_thickness)

def shrink_box(search_rectangle: list, blobs: list, x_shrink_rate: int=1, y_shrink_rate: int=1) -> list:
    """This function dynamically creates a search rectangle in which to look for a particle

    Args:
        search_rectangle (list<int>): a list of dimensions inside of which the camera will search for a particle of light [x-position, y-position, width, height]. Defaults to [].
            search_rectangle[0]: x coordinate of the search rectangle
            search_rectangle[1]: y coordinate of the search rectangle
            search_rectangle[2]: width of the search rectangle
            search_rectangle[3]: height of the search rectangle
        blobs (list<blob>): list of blobs detected by camera. Defaults to [].
        x_shrink_rate (int, optional): rate at which to decrease the box size (in pixels) in the horizontal direction. Defaults to 1.
        y_shrink_rate (int, optional): rate at which to decrease the box size (in pixels) in the horizontal direction. Defaults to 1.

    Returns:
        list: the modified search rectangle dimensions (x, y, width, height)
    """

    # These variables are the smallest that the search box can become, their size is in pixels. Therefore the smallest the search box can become
        # is 50x50 px
    smallest_width = 50
    smallest_height = 50

    # These variables are to store the average brightness in the x and y directions,
        # which will be used to hone in on where we suspect the brightest blob of light is
    avg_x = 0
    avg_y = 0
    # add up the center x and y coordinates of all the blobs and then divide those sums by the number of blobs found in the image.
        # the resulting coordinates are our best guess as to where a trapped particle is. They are the brightest point the camera can see.
    for blob in blobs:
        avg_x += blob.cx()
        avg_y += blob.cy()
    avg_x /= len(blobs)
    avg_y /= len(blobs)

    # the left and right most coordinates in view are the x value and the x value plus the width respectively.
    leftmost_coordinate = search_rectangle[0]
    rightmost_coordinate = search_rectangle[0] + search_rectangle[2]
    if search_rectangle[2] >= smallest_width: # if the current width is greater than or equal to the smallest width we have decided is acceptable
        if abs(leftmost_coordinate - avg_x) > abs(rightmost_coordinate - avg_x): # if the absolute value of the leftside coordinate is greater than the right
            search_rectangle[0] += x_shrink_rate # increase the value of the x coordinate (which in turn moves the left side of the rectangle inward)
            if search_rectangle[0] + search_rectangle[2] > VGA_WIDTH: # also, as a safety check, if the width of the search rectangle is greater than the maximum, decrease it.
                search_rectangle[2] -= 1
        else: # else, decrease the width (which moves the right side of the rectangle inward)
            search_rectangle[2] -= x_shrink_rate
    else: # else, do everything the if statement does minus the safety check.
        if abs(leftmost_coordinate - avg_x) > abs(rightmost_coordinate - avg_x):
            search_rectangle[0] += x_shrink_rate
        else:
            search_rectangle[0] -= x_shrink_rate

    # the comments for the height are the same for the width. reference the width comments to understand the height.
    topmost_coordinate = search_rectangle[1]
    bottommost_coordinate = search_rectangle[1] + search_rectangle[3]
    if search_rectangle[3] >= smallest_height:
        if abs(topmost_coordinate - avg_y) > abs(bottommost_coordinate - avg_y):
            search_rectangle[1] += y_shrink_rate
            if search_rectangle[1] + search_rectangle[3] > VGA_HEIGHT:
                search_rectangle[3] -= 1
        else:
            search_rectangle[3] -= y_shrink_rate
    else:
        if abs(topmost_coordinate - avg_y) > abs(bottommost_coordinate - avg_y):
            search_rectangle[1] += y_shrink_rate
        else:
            search_rectangle[1] -= y_shrink_rate

    return search_rectangle

def grow_box(search_rectangle: list) -> list:
    """This function increases the size of the search box when no blob that meets the requisite threshold is found.

    Args:
        search_rectangle (list): A list of the current search rectangle dimensions.
            search_rectangle[0]: x coordinate of the search rectangle
            search_rectangle[1]: y coordinate of the search rectangle
            search_rectangle[2]: width of the search rectangle
            search_rectangle[3]: height of the search rectangle

    Returns:
        list: the modified search rectangle dimensions
    """

    if search_rectangle[0] > 0:
        search_rectangle[0] -= 1
        search_rectangle [2] += 1

    if search_rectangle[1] > 0:
        search_rectangle[1] -= 1
        search_rectangle [3] += 1

    if (search_rectangle[2] + search_rectangle[0]) > 0:
        search_rectangle[2] += 1

    if (search_rectangle[3] + search_rectangle[1]) > 0:
        search_rectangle[3] += 1

    return search_rectangle

def configure_camera_io(pin1="P6", pin2="P5", camera_output_trap_status="P8", pi_input="P9", camera_output_status="P7") -> tuple:
    """This function configures what io pins will be used to communicate input and output to and from the camera and raspberry pi.

    Args:
        pin1 (str, optional): Location of pin1. Defaults to "P6".
        pin2 (str, optional): Location of pin2. Defaults to "P5".
        pin3 (str, optional): Output communication from the camera as to whether a particle has been trapped. Defaults to "P8".

    Returns:
        tuple<pyb.Pin>: Returns a tuple of pyb.Pin objects that correspond to the pins provided as arguments.
    """
    p = pyb.Pin(pin1, pyb.Pin.OUT_PP) # ? what is this pin supposed to communicate to the pi?
    p_arduino = pyb.Pin(pin2, pyb.Pin.OUT_PP) # ? what is this pin supposed to communicate to the pi?
    camera_output_trapped_or_not = pyb.Pin(camera_output_trap_status, pyb.Pin.OUT_PP) # ? what is this pin supposed to communicate to the pi?
    input_from_pi = pyb.Pin(pi_input, pyb.Pin.IN)
    camera_output_status = pyb.Pin(camera_output_status, pyb.Pin.OUT_PP)


    return (p, p_arduino, camera_output_trapped_or_not, input_from_pi, camera_output_status)

#####################################################################################?
# SET GAIN
# Change this value to adjust the gain. Try 10.0/0/0.1/etc.
GAIN_SCALE = 1.0
current_gain_in_decibels = sensor.get_gain_db()
print("Current Gain == %f db" % current_gain_in_decibels)
# Auto gain control (AGC) is enabled by default. Calling the below function
# disables sensor auto gain control. The additional "gain_db"
# argument then overrides the auto gain value after AGC is disabled.
sensor.set_auto_gain(False, gain_db=16)
print("New gain == %f db" % sensor.get_gain_db())
# The gain db ceiling maxes out at about 24 db for the OV7725 sensor.
# sensor.set_auto_gain(True, gain_db_ceiling=16.0) # Default gain.
response = sensor.__read_reg(0xBB) # ?
print("THIS IS A MYSTERIOUS SENSOR READING FROM DAYS & DEVELOPERS PAST: {}. NOT SURE WHAT IT MEANS...".format(response))
#####################################################################################?

"""GLOBAL VARS"""
INITIAL_CAMERA_X = 250
INITIAL_CAMERA_Y = 150
INITIAL_BOX_X = 150
INITIAL_BOX_Y = 20

BLOB_THRESHOLD = 2
MINIMUM_BRIGHTNESS_THRESHOLD = 245
MAXIMUM_BRIGHTNESS_POSSIBLE = 255
THRESHOLD = [MINIMUM_BRIGHTNESS_THRESHOLD, MAXIMUM_BRIGHTNESS_POSSIBLE]
configure_sensor() # consider making this the __init__() function of an intermediary camera class
p, p_arduino, camera_output_trapped_or_not, input_from_pi, camera_output_status = configure_camera_io()

run_camera = 1
# TODO: STOP USING MAGIC NUMBERS in the line below
search_roi = create_search_rectangle(INITIAL_CAMERA_X, INITIAL_CAMERA_Y, INITIAL_BOX_X, INITIAL_BOX_Y) # returns the dimensions for the initial rectangle within which the camera should seach for a trapped particle.
camera_output_trapped_or_not.low()

#count = 0

"""MAIN"""
while True:



    """ INFINITE LOOP


        the camera assumes 1, meaning that if nothing is connected to the pin you are reading from it will return a 1.

        high = 1
        low = 0

        camera_output_status.high() -> not searching (probably a particle is trapped, or some error has occured)
        camera_output_status.low() -> currently searching

        input_from_pi.high() -> run the camera (camera_output_status should be low)
        input_from_pi.low() -> don't run the camera

        camera_output_trapped_or_not.high() -> particle is trapped
        camera_output_trapped_or_not.low() -> no particle is trapped


        WHEN PARTICLE IS TRAPPED:
            camera_output_trapped_or_not = high
            camera_output_status = high

        WHEN NO PARTICLE IS TRAPPED:
            continue to grow box until a certain threshold,
            once threshold is passed, return:
                camera_output_trapped_or_not = low
                camera_output_status = high
    """

    run_camera = input_from_pi.value()

    if not run_camera:

        #count += 1
        snap_time= str(time.time())
        camera_output_status.low()

        image_snapshot = sensor.snapshot() # returns an 'image' object with information about what the camera sees at a given instant, methods are documented here: https://docs.openmv.io/library/omv.image.html
        image_snapshot.save("./pics/try" + snap_time + ".jpg")
        image_snapshot.draw_rectangle(search_roi)
        blobs_of_light_in_image_snapshot = image_snapshot.find_blobs([THRESHOLD], pixels_threshold=4, area_threshold=4, merge=False, roi=search_roi)



        if len(blobs_of_light_in_image_snapshot) > 1 and len(blobs_of_light_in_image_snapshot) <= BLOB_THRESHOLD: # if multiple particles of light whose brightness is greater than MINIMUM_BRIGHTNESS_THRESHOLD are found
            search_roi = shrink_box(search_rectangle=search_roi, blobs=blobs_of_light_in_image_snapshot, x_shrink_rate=5, y_shrink_rate=4) # shrink the search box to hone in on the brightest particles

            # DRAW A CROSS ON EACH PARTICLE FOUND SO THE USER CAN SEE PARTICLES THE CAMERA HAS IDENTIFIED IN THE OPEN MV IDE
            mark_particles_in_frame(blobs_of_light_in_image_snapshot, image_snapshot)
            print("TRAPPED! {} LIGHT BLOB(S) FOUND.".format(len(blobs_of_light_in_image_snapshot)))

            camera_output_trapped_or_not.high() # indicate to pi that a particle is trapped
            camera_output_status.high() # indicate to the pi that the camera is no longer searching bc it has found a particle and is tracking that
            # time.sleep(0.5)

            width = search_roi[2]
            height = search_roi[3]
            if (width <= 50) and (height <= 50):
                search_roi = create_search_rectangle(INITIAL_CAMERA_X, INITIAL_CAMERA_Y, INITIAL_BOX_X, INITIAL_BOX_Y) # returns the dimensions for the initial rectangle within which the camera should seach for a trapped particle.

        else:
            print("NO TRAP")
            search_roi = grow_box(search_rectangle=search_roi) # since there are not blobs of light in the camera's field of view, increase the size of the field of view

            camera_output_trapped_or_not.low() # indicate to the pi that no particle is currently trapped.

            width = search_roi[2]
            height = search_roi[3]
            if (width > 300) and (height > 200):
                # TODO: STOP USING MAGIC NUMBERS in the line below
                search_roi = create_search_rectangle(INITIAL_CAMERA_X, INITIAL_CAMERA_Y, INITIAL_BOX_X, INITIAL_BOX_Y) # returns the dimensions for the initial rectangle within which the camera should seach for a trapped particle.

                camera_output_status.high() # camera is not searching for a particle bc
                # time.sleep(0.5)

        print("PI COMMAND: {}".format(input_from_pi.value()))
        #print("BY ORDER OF THE PI, RUNNING TEST: {}".format(count))


    else:
        camera_output_trapped_or_not.low() # indicate to the pi that there is no trap bc camera is not looking for one
        camera_output_status.high() # indicate to the pi that the camera isn't seaching bc it has no orders


    print("camera_output_status: {}\ninput_from_pi: {}\ncamera_output_trapped_or_not: {}\n".format(camera_output_status.value(), input_from_pi.value(), camera_output_trapped_or_not.value()))