import math
import cv2
import numpy
from cscore import CameraServer
from app.classes.SettingsManager import SettingsManager
from ..handlers.SocketHandler import send_all_async
from multiprocessing import Process
import threading
import zmq
import asyncio
import time
from networktables import NetworkTables
import networktables
from .VisionHandler import VisionHandler


class CameraHandler:
    def __init__(self, cam_name, port):
        #settings vars up for vision loop
        self.cs = CameraServer.getInstance()
        self.settings_manager = SettingsManager()
        self.vision_handler = VisionHandler()
        self.port = port
        self.cam_name = cam_name
        self.image = None
        self.p_image = None
        self.table = None
        self.nt_data = {'valid': False}
        self.time_stamp = 0

    def run(self):
        #starting main thread
        threading.Thread(target=self.thread_proc).start()

    def thread_proc(self):
        self.settings_manager.cams_curr_pipeline[self.cam_name] = "pipeline0"
        pipeline = self.settings_manager.cams[self.cam_name]["pipelines"][self.settings_manager.cams_curr_pipeline[self.cam_name]]
        FOV = self.settings_manager.cams[self.cam_name]["FOV"]

        def change_camera_values(pipeline):
            self.settings_manager.usb_cameras[self.cam_name].setBrightness(pipeline['brightness'])
            self.settings_manager.usb_cameras[self.cam_name].setExposureManual(pipeline['exposure'])
            self.settings_manager.usb_cameras[self.cam_name].setWhiteBalanceAuto()

        def pipeline_listener(table, key, value, is_new):
            asyncio.set_event_loop(asyncio.new_event_loop())
            if value in self.settings_manager.cams[self.cam_name]['pipelines'].keys():
                self.settings_manager.cams_curr_pipeline[self.cam_name] = value
                change_camera_values(pipeline)
                if self.cam_name == self.settings_manager.general_settings['curr_camera']:
                    self.settings_manager.general_settings['curr_pipeline'] = value
                    update_settings = self.settings_manager.get_curr_pipeline()
                    update_settings['curr_pipeline'] = self.settings_manager.general_settings["curr_pipeline"]
                    send_all_async(update_settings)
            else:
                self.table.putString('Pipeline', self.settings_manager.cams_curr_pipeline[self.cam_name])

        def mode_listener(table, key, value, is_new):
            if value:
                change_camera_values({
                    'brightness': 25,
                    'exposure': 15
                })
            else:
                change_camera_values(pipeline)
        #setting up network table
        self.table = NetworkTables.getTable("/Chameleon-Vision/" + self.cam_name)
        #init values for pipeline and driver mode
        self.table.putString('Pipeline', self.settings_manager.cams_curr_pipeline[self.cam_name])
        self.table.putBoolean('Driver_Mode', False)
        self.table.addEntryListenerEx(pipeline_listener, key="Pipeline",
                                 flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
        self.table.addEntryListenerEx(mode_listener, key="Driver_Mode",
                                 flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)

        # getting video from current camera
        cv_sink = self.cs.getVideo(camera=self.settings_manager.usb_cameras[self.cam_name])

        width = self.settings_manager.cams[self.cam_name]["video_mode"]["width"]
        height = self.settings_manager.cams[self.cam_name]["video_mode"]["height"]

        # setting up a video server for camera
        cv_publish = self.cs.putVideo(name=self.cam_name, width=width, height=height)
        # saving camera port in cam name dict for usage in client
        self.settings_manager.cams_port[self.cam_name] = self.cs._sinks['serve_' + self.cam_name].getPort()

        # setting up a zmq connection to the opencv subprocess
        context = zmq.Context()
        socket = context.socket(zmq.PAIR)
        socket.bind('tcp://*:%s' % str(self.port))

        # starting the process with initial values
        p = Process(target=self.camera_process, args=(self.cam_name, self.port, FOV))
        p.start()

        change_camera_values(pipeline)

        def _publish_thread():
            #getting image values and publishing process image and data
            self.image = numpy.zeros(shape=(width, height, 3), dtype=numpy.uint8)
            self.p_image = self.image
            while True:
                try:
                    self.time_stamp, self.image = cv_sink.grabFrame(self.image)
                    cv_publish.putFrame(self.p_image)
                    self.table.putBoolean('valid', self.nt_data['valid'])
                    # check if point is valid
                    if self.nt_data['valid']:
                        # send the point using network tables
                        self.table.putNumber('pitch', self.nt_data['pitch'])
                        self.table.putNumber('yaw', self.nt_data['yaw'])
                    self.table.putNumber('time_stamp', self.nt_data['time_stamp'])
                    self.table.putNumber('fps', self.nt_data['fps'])
                        # if the selected camera in ui is this cam send the point to the ui
                except:
                    pass

        def _socket_thread():
            #publishing to websocket at slower interval
            asyncio.set_event_loop(asyncio.new_event_loop())
            while True:
                time.sleep(0.1)
                if self.settings_manager.general_settings['curr_camera'] == self.cam_name:
                    try:
                        send_all_async({
                            'raw_point': self.nt_data['raw_point'],
                            'point': {
                                'pitch': self.nt_data['pitch'],
                                'yaw': self.nt_data['yaw'],
                                'fps': self.nt_data['fps']
                            }
                        })
                    except:
                        pass

        threading.Thread(target=_publish_thread).start()
        threading.Thread(target=_socket_thread).start()

        while True:
            #sending and reciving data from opencv sub process
            pipeline = self.settings_manager.cams[self.cam_name]["pipelines"][
                self.settings_manager.cams_curr_pipeline[self.cam_name]]

            socket.send_json(dict(
                pipeline=pipeline,
                driver_mode=self.table.getBoolean('Driver_Mode', False)
            ), zmq.SNDMORE)

            socket.send_pyobj((self.time_stamp,self.image))
            self.p_image = socket.recv_pyobj()
            self.nt_data = socket.recv_json()

    def camera_process(self, cam_name, port, FOV):
        from fractions import Fraction
        #calc fov
        diagonalView = math.radians(FOV)

        width = self.settings_manager.cams[cam_name]["video_mode"]["width"]
        height = self.settings_manager.cams[cam_name]["video_mode"]["height"]
        centerX = (width / 2) - .5
        centerY = (height / 2) - .5
        cam_area = width * height

        aspect_fraction = Fraction(width, height)
        horizontal_ratio = aspect_fraction.numerator
        vertical_ratio = aspect_fraction.denominator

        horizontalView = math.atan(math.tan(diagonalView / 2) * (horizontal_ratio / diagonalView)) * 2
        verticalView = math.atan(math.tan(diagonalView / 2) * (vertical_ratio / diagonalView)) * 2

        H_FOCAL_LENGTH = width / (2 * math.tan((horizontalView / 2)))
        V_FOCAL_LENGTH = height / (2 * math.tan((verticalView / 2)))
        #setting up zmq socket
        context = zmq.Context()
        socket = context.socket(zmq.PAIR)
        socket.connect('tcp://localhost:%s' % str(port))
        #setting up filter countours class
        filter_contours = self.vision_handler.Filter_Contours(center_x=centerX, center_y=centerY)

        x = 1
        counter = 0
        start_time = time.time()
        fps = 0

        while True:
            obj = socket.recv_json()
            curr_pipeline = obj['pipeline']
            driver_mode = obj['driver_mode']
            time_stamp, image = socket.recv_pyobj()
            if curr_pipeline['orientation'] == "Inverted":
                M = cv2.getRotationMatrix2D((width / 2, height / 2), 180, 1)
                image = cv2.warpAffine(image, M, (width, height))
            if not driver_mode:
                hsv_image = self.vision_handler._hsv_threshold(curr_pipeline["hue"],
                                                curr_pipeline["saturation"], curr_pipeline["value"],
                                                image, curr_pipeline["erode"], curr_pipeline["dilate"])
                # if table.getBoolean("Driver_Mode", False):
                contours = self.vision_handler.find_contours(hsv_image)
                filtered_contours = filter_contours.filter_contours(input_contours=contours, area=curr_pipeline['area'],
                                                                    ratio=curr_pipeline['ratio'],
                                                                    extent=curr_pipeline['extent'],
                                                                    sort_mode=curr_pipeline['sort_mode'], cam_area=cam_area,
                                                                    target_grouping=curr_pipeline['target_group'],
                                                                    target_intersection=
                                                                    curr_pipeline['target_intersection'])

                final_contour = self.vision_handler.output_contour(filtered_contours)

                try:
                    center = final_contour[0]
                    if curr_pipeline["M"] == 1 and curr_pipeline["B"] == 0:
                        center_x = centerX
                        center_y = centerY
                    else:
                        center_x = (center[1] - curr_pipeline['B']) / curr_pipeline["M"]
                        center_y = (center[0] * curr_pipeline["M"]) + curr_pipeline["B"]
                    pitch = self.vision_handler.calculate_pitch(pixel_y=center[1], center_y=center_y, v_focal_length=V_FOCAL_LENGTH)
                    yaw = self.vision_handler.calculate_yaw(pixel_x=center[0], center_x=center_x, h_focal_length=H_FOCAL_LENGTH)
                    valid = True
                except IndexError:
                    center = None
                    pitch = None
                    yaw = None
                    valid = False

                if curr_pipeline['is_binary']:
                    draw_image = hsv_image
                else:
                    draw_image = image

                res = self.vision_handler.draw_image(input_image=draw_image, contour=final_contour)
            else:
                res = image
                center = None
                pitch = None
                yaw = None
                valid = False

            socket.send_pyobj(res)
            socket.send_json(dict(
                pitch=pitch,
                yaw=yaw,
                valid=valid,
                raw_point=center,
                fps=fps,
                time_stamp=time_stamp
            ))
            counter += 1
            if (time.time() - start_time) > x:
                fps = (counter / (time.time() - start_time))
                counter = 0
                start_time = time.time()
