#! python3
# This file is part of the OpenMV project.


import sys, os
import numpy as np
import pygame
import pyopenmv
from time import sleep

import config

dir_path = os.path.dirname(os.path.realpath(__file__))

# cam code loader
with open(dir_path + '/cam.py', 'r') as contentFile:
    script = contentFile.read()

# init pygame
pygame.init()
programIcon = pygame.image.load(dir_path + '/icon.png')
pygame.display.set_icon(programIcon)
pygame.display.set_caption('EkoSłupek')

connected = False
pyopenmv.disconnect()
for i in range(10):
    try:
        # opens CDC port.
        # Set small timeout when connecting
        pyopenmv.init(config.portname, baudrate=921600, timeout=0.050)
        connected = True
        break
    except Exception as e:
        connected = False
        sleep(0.100)

if not connected:
    print ( "Failed to connect to OpenMV's serial port.\n\n")
    sys.exit(1)

# Set higher timeout after connecting for lengthy transfers.
pyopenmv.set_timeout(1*2) # SD Cards can cause big hicups.
pyopenmv.stop_script()
pyopenmv.enable_fb(True)
pyopenmv.exec_script(script)

# init screen
running = True
Clock = pygame.time.Clock()
font = pygame.font.SysFont("monospace", 15)

while running:
    Clock.tick(60)

    # read framebuffer
    fb = pyopenmv.fb_dump()
    if fb != None:
        # scale
        width = fb[0] * config.scale
        height = fb[1] * config.scale
        # create image from RGB888
        image = pygame.image.frombuffer(fb[2].flat[0:], (fb[0], fb[1]), 'RGB')
        image = pygame.transform.scale(image, (width, height))
        # TODO check if res changed
        screen = pygame.display.set_mode((width, height), pygame.DOUBLEBUF, 32)
        # blit stuff
        screen.blit(image, (0, 0))
    
        # update display
        pygame.display.flip()

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
             running = False
        elif event.type == pygame.KEYDOWN:
            if event.key == pygame.K_ESCAPE:
                running = False
            if event.key == pygame.K_c:
                pygame.image.save(image, dir_path + "/capture.png")

pygame.quit()
pyopenmv.stop_script()