#!/usr/bin/env python
# -*- coding: utf-8 -*-
import wx
import numpy as np

# ROS imports
import rospy
import std_msgs.msg
from sensor_msgs.msg import Image, CompressedImage, NavSatFix, Imu
from nav_msgs.msg import Odometry
from diagnostic_msgs.msg import DiagnosticStatus

# ADAPT imports
from rc_genicam_camera.srv import GetGenICamParameter, SetGenICamParameter


success_enum = {}
success_enum[-1] = 'INVALID_ARGUMENT'
success_enum[-2] = 'UNINITIALIZED'
success_enum[-3] = 'INTERNAL_TIMEOUT'
success_enum[-4] = 'SENSOR_TIMEOUT'
success_enum[-5] = 'REQUEST_TO_DATABASE_FAILED'
success_enum[-6] = 'REQUEST_TO_TF_MODULE_FAILED'
success_enum[-7] = 'IO_ERROR'
success_enum[-8] = 'INVALID_REQUEST'
success_enum[-9] = 'INVALID_LICENSE'


class RCGenicamCameraController(object):
    """GUI's convenience interface to camera running on rc_genicam_camera node.

    """
    def __init__(self, parent, get_service_topic, set_service_topic):
        """
        """
        self.parent = parent
        self._get_service = rospy.ServiceProxy(get_service_topic,
                                               GetGenICamParameter,
                                               persistent=False)

        self._set_service = rospy.ServiceProxy(set_service_topic,
                                               SetGenICamParameter,
                                               persistent=False)

    def unregister(self):
        pass

    def get_service(self, command, call_summary_txt):
        return self.service(self._get_service, command, call_summary_txt)

    def set_service(self, command, call_summary_txt):
        return self.service(self._set_service, command, call_summary_txt)

    def service(self, service, command, call_summary_txt):
        """Send a service command.

        :param service: Set or get service.
        :type service: rospy.ServiceProxy

        :return: First element is a bool indicating whether the command was
            executed successfully.
        :rtype 2-tuple
        """
        print('sending command: ', command)
        try:
            success = True
            resp = service(command)
            value = resp.value
            if resp.SUCCESS == 0:
                success = True
            else:
                success = False
                # See https://github.com/roboception/rc_genicam_camera/blob/master/msg/ReturnValue.msg
                err_msg = success_enum[resp.SUCCESS]

        except rospy.service.ServiceException, e:
            success = False
            err_msg = '\'%s\' failed because \'%s\'' % (call_summary_txt, e)

        if not success:
            self.emit_error_dialog(err_msg)
            value = None

        return success, value

    def emit_error_dialog(self, err_msg):
        icon = wx.ICON_ERROR
        dlg = wx.MessageDialog(self.parent, err_msg, 'Info', wx.OK | icon)
        dlg.ShowModal()
        dlg.Destroy()

    def get_auto_exposure_time_upper_limit(self, value):
        """Set exposure time upper limit in milliseconds.
        """
        #print('AutoExposureExposureTimeUpperLimit', value)
        return self.get_service('AutoExposureExposureTimeUpperLimit',
                                'Auto Exposure Time Upper Limit')

    def get_auto_exposure_time_lower_limit(self, value):
        """Set exposure time lower limit in milliseconds.
        """

        return self.get_service('AutoExposureExposureTimeLowerLimit',
                                'Auto Exposure Time Lower Limit')

    def get_auto_gain_upper_limit(self, value):
        """Set exposure gain upper limit in milliseconds.
        """

        return self.get_service('AutoExposureGainUpperLimit',
                                'Auto Gain Upper Limit')

    def set_auto_exposure_time_upper_limit(self, value):
        """Set exposure time upper limit in milliseconds.
        """
        try:
            value = int(np.round(float(value)*1000))
        except ValueError:
            self.emit_error_dialog('Invalid value \'%s\'' % value)
            return False, None

        #print('AutoExposureExposureTimeUpperLimit', value)
        return self.set_service('AutoExposureExposureTimeUpperLimit=%i' % value,
                                'Auto Exposure Time Upper Limit=%i' % value)

    def set_auto_exposure_time_lower_limit(self, value):
        """Set exposure time lower limit in milliseconds.
        """
        try:
            value = int(np.round(float(value)*1000))
        except ValueError:
            self.emit_error_dialog('Invalid value \'%s\'' % value)
            return False, None

        return self.set_service('AutoExposureExposureTimeLowerLimit=%i' % value,
                                'Auto Exposure Time Lower Limit=%i' % value)

    def set_auto_gain_upper_limit(self, value):
        """Set gain upper limit in milliseconds.
        """
        try:
            value = int(np.round(float(value)*1000))
        except ValueError:
            self.emit_error_dialog('Invalid value \'%s\'' % value)
            return False, None

        return self.set_service('AutoExposureGainUpperLimit=%i' % value,
                                'Auto Exposure Gain Upper Limit=%i' % value)

    def set_ptp_enable(self):
        #print('GevIEEE1588=1')
        self.set_service('GevIEEE1588=1', 'PTP Enable')

    def get_ptp_status(self):
        return self.get_service('GevIEEE1588Status')
