#!/usr/bin/env python3

from typing import Union, overload
from picamera2 import Picamera2, MappedArray, Metadata
from pprint import pprint
import json
import cv2
import time

#import numpy as np
#import argparse
#import sys
#import csv
#import matplotlib.pyplot as plt

picam2 = Picamera2()
pprint(picam2.sensor_modes)

items = [
    "ColourCorrectionMatrix",
    "Saturation",
    "Contrast",
    "Sharpness",
    "Brightness",
    "NoiseReductionMode",
    "AeEnable",
    "AeMeteringMode",
    "AeConstraintMode",
    "AeExposureMode",
    "AwbEnable",
    "AwbMode",
    "ExposureValue",
    "ExposureTime",
    "AnalogueGain",
    "ColourGains",
    "ScalerCrop",
    "FrameDurationLimits",
    "FrameRate"
]

settings = {'AeEnable': 0, 'AwbEnable': 0, 'NoiseReductionMode': 0, 'ExposureTime': 10000, 'AnalogueGain': 1, 'FrameRate': picam2.sensor_modes[0]['fps']}
caps = {"FrameRate": (1, picam2.sensor_modes[0]['fps'], None)}
fps_ist = 0
fps_soll = 0


def load_settings():
    _result = {}
    try:
        with open("settings.json", "r") as fp:
            _result = json.load(fp)
    except Exception:
        pass

    return _result


def save_settings(_settings):
    with open("settings.json", "w") as fp:
        json.dump(_settings, fp, indent=0)


def apply_settings(_settings):
    for item in _settings:
        c = caps[item]
        value = _settings[item]
        if value < c[0] or value > c[1]:
            raise Exception(f"Invalid Value for {item}")

        picam2.set_controls({item: value})


def clamp_and_store(item: str, value):
    clamped_value = value
    c = caps[item]
    clamped_value = max(c[0], min(c[1], value))
    settings[item] = clamped_value

    return clamped_value

   
def process(request):

    timestamp = time.strftime("%Y-%m-%d %X")

    # read metadata from request
    metadata = Metadata(request.get_metadata())

    # Clamp our shutter value to minimum of FrameDuration
    caps["ExposureTime"] = (caps["ExposureTime"][0], metadata.FrameDuration)

    with MappedArray(request, "main") as m:
        # Display timestamp on frame
        cv2.putText(m.array, timestamp, (0, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

        if metadata is not None:
            # Display exposure time
            cv2.putText(m.array, f"ExposureTime  : {metadata.ExposureTime:.1f}/{shutter:.1f}", (20, m.array.shape[0] - 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 240, 0), 2)

            # Display frame time
            cv2.putText(m.array, f"FrameDuration : {metadata.FrameDuration:.1f}", (20, m.array.shape[0] - 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 240, 0), 2)

        # Display FPS on frame
        cv2.putText(m.array, f"Frame rate    : {fps_ist:.1f}/{fps_soll:.1f}", (20, m.array.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 240, 0), 2)

shutter_keys = {'w': -1000, 's': -100, 'x': -1, 'e': 1000, 'd': 100, 'c': 1}
gain_keys = {'h': -1, 'j': 1}
fps_keys = {'r': -1, 'f': -0.1, 'v': -0.01, 't': 1, 'g': 0.1, 'b': 0.01}

def keypress_changed(key_defs: dict, key: int):
    _res = None
    try:
        _res = key_defs[str(chr(key))]
    except KeyError:
        pass

    return _res

if __name__ == '__main__':
    for item in items:
        try:
            caps[item] = (picam2.camera_controls[item][0], picam2.camera_controls[item][1])
        except KeyError:
            print(f"{item}: Not implemented")

    # Config
    camera_config = picam2.create_preview_configuration({'format': 'XRGB8888', 'size': picam2.sensor_modes[0]['size']})
    camera_config = picam2.create_preview_configuration({'format': 'XRGB8888', 'size': (800, 600)})
    picam2.configure(camera_config)

    # Load and apply our settings
    settings |= load_settings()
    apply_settings(settings)

    # Init vars
    shutter = settings['ExposureTime']
    gain = settings['AnalogueGain']
    fps_soll = settings['FrameRate']

    # Print status
    pprint(f"Capabilities: {caps}")
    pprint(f"Settings: {settings}")
    pprint(f"Camera config: {picam2.camera_configuration()}")

    # Register OCV processing callback
    picam2.pre_callback = process

    # Start
    picam2.start()
    
    while True:

        # Start timer
        timer = cv2.getTickCount()

        # Capture frame
        frame = picam2.capture_array("main")

        # Process the frame using OpenCV operations
        cv2.imshow('Frame', frame)

        # Exit the loop on pressing 'q'
        key = cv2.pollKey() & 0xFF
        if key == ord('q'):
            break

        control_changed = False
        v = keypress_changed(shutter_keys, key)
        if v is not None:
            control_changed = True
            shutter = clamp_and_store('ExposureTime', shutter + v)
            print(f"Shutter: {shutter}")

        v = keypress_changed(gain_keys, key)
        if v is not None:
            control_changed = True
            gain = clamp_and_store('AnalogueGain', gain + v)
            print(f"Gain: {gain}")

        v = keypress_changed(fps_keys, key)
        if v is not None:
            control_changed = True
            fps_soll = clamp_and_store('FrameRate', fps_soll + v)
            print(f"FPS: {fps_soll}")

        if control_changed:
            # Write contrrols to camera
            with picam2.controls as controls:
                controls.ExposureTime = shutter
                controls.AnalogueGain = gain
                controls.FrameRate = fps_soll

        # Calculate Frames per second (FPS)
        fr = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
        alpha = 0.2
        fps_ist = (1-alpha)*fps_ist + alpha*fr


    # Release the capture and close windows
    save_settings(settings)
    cv2.destroyAllWindows()