#!/usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import division, print_function
import os
import sys
import threading
import time
import datetime
import json
import copy
from functools import partial
from six import StringIO
from osgeo import osr, gdal

# GUI imports
import wx
from wx.lib.wordwrap import wordwrap

# Vision / math
import cv2
import numpy as np
from PIL import Image as PILImage

# Geo
import pygeodesy
import shapely
import shapely.geometry
import shapefile

# ROS imports
import rospy
import std_msgs.msg
from sensor_msgs.msg import Image, CompressedImage
from nav_msgs.msg import Odometry
from diagnostic_msgs.msg import DiagnosticStatus
from cv_bridge import CvBridge, CvBridgeError
from tf.transformations import euler_matrix

# ADAPT imports
from custom_msgs.srv import RequestCompressedImageView, \
    RequestImageMetadata, RequestImageView
from sensor_models.nav_conversions import llh_to_enu, enu_to_llh, \
    ned_quat_to_enu_quat, enu_quat_to_ned_quat
import form_builder_output
import form_builder_output_imagery_inspection
import form_builder_output_hot_key_list
import form_builder_output_collection_mode
import form_builder_output_system_control
import form_builder_output_base_layer
import gui_utils
from rc_genicam_camera_controller import RCGenicamCameraController
from spatial_ins import SpatialINSInterface
from recorder.srv import start_recording, stop_recording, reboot, shutdown

ros_immediate = rospy.Duration(nsecs=1)

# Instantiate CvBridge
bridge = CvBridge()

# Maximum megapixels to request from image server. The image shown in the GUI,
# which maybe be a downsampled version requested from the nexus, will not
# exceed this value of Megapixels.
MAX_MPIX = 0.2e6

G_MAX_FRAME_RATE = 2

# Throttle the GUI to only ask for updates no quicker than this rate.
GUI_UPDATE_THROTTLE = 20

# The GUI will not bother asking again for imagery that it has already received
# in order to reduce bandwidth needs. But, if playing imagery from a bag on
# loop, the GUI would then stop updating on the loop. This parameters overrides
# the check so that the GUI always updates.
UPDATE_WITH_OLD_IMAGERY = True

# Show satured pixels as red.
show_satured_pixels = False

# CLAHE clip limit.
contrast_strength = 2

G_time_note_started = None  # type: datetime.datetime

TEXTCTRL_GRAY = (255, 23, 23)
TEXTCTRL_WHITE = (255, 255, 255)
TEXTCTRL_DARK = (20, 20, 20)
APP_GRAY = (220, 218, 213)  # Default application background
FLAT_GRAY = (200, 200, 200)
COLLECT_GREEN = (55, 120, 25)
SHAPE_COLLECT_BLUE = (52, 100, 212)  # shape file is set and "primed" but not currently in it
DIAG_OK_GREEN = (0, 120, 0)
VERDANT_GREEN = (0, 170, 45)
BRIGHT_GREEN = (0, 255, 0)
BRIGHT_RED = (255, 0, 0)
ERROR_RED = (192, 0, 0)
WARN_AMBER = (255, 192, 90)
WARN_AMBER2 = (224, 128, 0)
WARN_AMBER2 = (224, 128, 0)
WTF_PURPLE = (96, 48, 192)

# Diagnostic State Colors
DIAG_OK_GREEN = (0, 120, 0)
DIAG_WARN_AMBER = (255, 192, 90)
DIAG_ERROR_RED = (120, 0, 0)
DIAG_STALE_BLUE = (52, 100, 212)

HERE_DIR = os.path.dirname(os.path.realpath(__file__))
PKG_DIR = os.path.realpath(os.path.join(HERE_DIR, '../../..'))

# Location of the configuration file dictionary.
config_filename = '%s/config/system_control_panel_settings.json' % PKG_DIR

# Location of the configuration file dictionary.
camera_config_filename = '%s/config/camera_configurations.json' % PKG_DIR

# create from template if it doesn't exist
if not os.path.isfile(config_filename):
    gui_utils.make_path(config_filename, from_file=True)
    with open(os.path.join(PKG_DIR, 'config/default_system_control_panel_settings.json'), 'r') as infile:
        with open(config_filename, 'w') as outfile:
            outfile.write(infile.read())
            print('Created config from scratch: {}'.format(config_filename))

# Location of the geod file.
geod_filename = os.path.join(PKG_DIR, 'geods/egm84-15.pgm')

geod = pygeodesy.geoids.GeoidPGM(geod_filename)


license_str = ''.join(['Copyright 2021 by Kitware, Inc.\n',
'Licensed under the Apache License, Version 2.0 (the "License");\n\n',
'you may not use this file except in compliance with the License.',
'You may obtain a copy of the License at',
'\n\n',
'http://www.apache.org/licenses/LICENSE-2.0',
'\n\n',
'Unless required by applicable law or agreed to in writing, software',
'distributed under the License is distributed on an "AS IS" BASIS',
'WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.',
'See the License for the specific language governing permissions and',
'limitations under the License.'])


def set_diagnostic_level_symbol(static_text_obj, level):
    """Sets a static text object to an ☑ or ❎.

    :param value: Integer value for state.
        OK - level=0, symbol=☑
        WARN - level=1, symbol=⚠
        ERROR - level=2, symbol=❎
        STALE - level=3, symbol=�
    :type value: byte OK=0

    """
    if level is None:
        return

    if level == 0:
        static_text_obj.SetLabel(u'☑')
        static_text_obj.SetForegroundColour(DIAG_OK_GREEN)
        #font = wx.Font(18, wx.MODERN, wx.NORMAL, wx.NORMAL, False, u'Consolas')
    elif level == 1:
        static_text_obj.SetLabel(u'⚠')
        static_text_obj.SetForegroundColour(WARN_AMBER)
        #font = wx.Font(18, wx.MODERN, wx.NORMAL, wx.NORMAL, False, u'Consolas')
    elif level == 2:
        static_text_obj.SetLabel(u'☒')
        static_text_obj.SetForegroundColour(DIAG_ERROR_RED)
        #font = wx.Font(18, wx.MODERN, wx.NORMAL, wx.NORMAL, False, u'Consolas')
    elif level == 3:
        static_text_obj.SetLabel(u'�')
        static_text_obj.SetForegroundColour(DIAG_STALE_BLUE)
        #font = wx.Font(18, wx.MODERN, wx.NORMAL, wx.NORMAL, False, u'Consolas')
    else:
        raise ValueError('invalid value for \'level\'=%' % level)

    #static_text_obj.SetFont(font)


def stretch_image_contrast(img, stretch_strength):
    if stretch_strength == 0:
        return img

    img = img.astype(np.float32)
    img -= np.percentile(img.ravel(), 0.1)
    img[img < 0] = 0
    img /= np.percentile(img.ravel(), 99.9)
    img[img > 1] = 1
    img = np.round(img*255).astype(np.uint8)

    clahe = cv2.createCLAHE(clipLimit=stretch_strength, tileGridSize=(8, 8))
    clahe_saturation = cv2.createCLAHE(clipLimit=stretch_strength,
                                       tileGridSize=(8, 8))
    if img.ndim == 3:
        hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
        hsv[:, :, 2] = clahe.apply(hsv[:, :, 2])

        if stretch_strength > 0:
            hsv[:, :, 1] = clahe_saturation.apply(hsv[:, :, 1])

        return cv2.cvtColor(hsv, cv2.COLOR_HSV2RGB)
    else:
        return clahe.apply(img)


def verbose_call_service(srv, **kwargs):
    print('Calling {}\n{}'.format(srv.resolved_name, kwargs))
    return srv(**kwargs)


class UpdateImageThread(threading.Thread):
    """This defines a thread to continually check to see if new imagery is
    available on the remote image server.

    This thread manages requests to a remote image server using
    ROS RequestImageView service calls. When first launched, a ROS
    RequestImageMetadata service is called to extract the resolution and
    encoding of the imagery available on the remote server.

    Then, a loop runs calling get_new_raw_image, which attempts to make a
    RequestImageView service call. The remote image server blocks until a new
    image on that channel is available.

    """
    def __init__(self, parent, srv_topic, metadata_srv_topic,
                 compressed=False):
        """
        :param srv_topic: ROS topic for RequestImageView service to provide
            the imagery needed.
        :type srv_topic: str

        :param metadata_srv_topic: ROS topic for RequestImageMetadata
            service to provide metadata about the imagery associated with
            srv_topic.
        :type metadata_srv_topic: str

        """
        threading.Thread.__init__(self)
        self.daemon = True
        self._parent = parent
        self._raw_image_height = None
        self._raw_image_width = None
        self._ros_srv_topic = srv_topic
        self._ros_metadata_srv_topic = metadata_srv_topic
        self._compressed = compressed
        self._stop = False
        self._last_header = None

    @property
    def last_header(self):
        return self._last_header

    def run(self):
        """Overrides Thread.run. Don't call this directly its called internally
        when you call Thread.start().

        """
        rospy.loginfo('Subscribing to service: \'%s\'' % self._ros_srv_topic)
        self._image_service = rospy.ServiceProxy(self._ros_srv_topic,
                                                 RequestImageView,
                                                 persistent=False)
        self._compress_image_service = rospy.ServiceProxy('%s/compressed' %
                                                          self._ros_srv_topic,
                                                          RequestCompressedImageView,
                                                          persistent=False)
        self._metadata_service = rospy.ServiceProxy(self._ros_metadata_srv_topic,
                                                    RequestImageMetadata,
                                                    persistent=False)

        while True:
            # this will spin at like 100Hz if you let it
            if self._stop: return None  # Check for a request to stop.
            try:
                print('Requesting image metadata on service \'%s\'' %
                      self._ros_metadata_srv_topic)
                resp = self._metadata_service()
                if not resp.success:
                    time.sleep(0.1)
                    continue

                if resp.height == 0 or resp.width == 0:
                    time.sleep(0.1)
                    continue

                self._raw_image_height = resp.height
                self._raw_image_width = resp.width
                print('Successfuly received image size % x %i on service '
                      '\'%s\'' % (resp.width, resp.height,
                                  self._ros_metadata_srv_topic))
            except rospy.service.ServiceException:
                time.sleep(0.1)
                continue

            break

        while True:
            if self._stop: return None  # Check for a request to stop.
            try:
                self.get_new_raw_image()
            except AssertionError:
                pass # wx noise
            except Exception as e:
                exc_type, exc_obj, exc_tb = sys.exc_info()
                fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
                msg = "{}:{}\n{}: {}".format(fname, exc_tb.tb_lineno, exc_type.__name__, e)
                print(msg)

            time.sleep(1/GUI_UPDATE_THROTTLE)

    def get_homography(self):
        """Return homography to warp from panel to raw-image coordinates.

        """
        raise NotImplementedError('This is an abstract base class')

    def get_new_raw_image(self, release=0):
        """Attempts RequestImageView service call to update raw imagery.

        This method considers the panel, where the imagery will be place,
        height and width and requests a downsampled version of the raw imagery
        that best matches the panel. This allows only the pixels needed to be
        transported from the image server.

        :param: release - enum which tells the service how to release any image threads, or return immediately
        without blocking
        see RequestImageView.srv service
        """
        if UPDATE_WITH_OLD_IMAGERY or self.last_header is None:
            tlast = -1
        else:
            tlast = self.last_header.stamp.to_sec()

        try:
            homography, output_height, output_width = self.get_homography()

            # Invert and flatten
            homography_ = tuple(np.linalg.inv(homography).ravel())

            if self._compressed:
                #print('Requesting image update on topic \'%s/compressed\'' % self._ros_srv_topic)
                resp = self._compress_image_service(homography=homography_,
                                                    output_height=output_height,
                                                    output_width=output_width,
                                                    interpolation=0,
                                                    antialias=False,
                                                    newer_than_time=tlast)

            else:
                #print('Requesting image update on topic \'%s\'' % self._ros_srv_topic)
                resp = self._image_service(homography=homography_,
                                           output_height=output_height,
                                           output_width=output_width,
                                           interpolation=0, antialias=False,
                                           newer_than_time=tlast)

            if not resp.success:
                if self._stop: return None  # Check for a request to stop.
                wx.CallAfter(self._parent.update_status_msg, 'No Update')
                return

            if self._stop: return None  # Check for a request to stop.
            image_msg = resp.image
            self._last_header = image_msg.header
            if self._compressed:
                sio = StringIO.StringIO(image_msg.data)
                im = PILImage.open( sio )
                image = np.array( im )
                image = image.copy()
            else:
                try:
                    # Convert your ROS Image message to OpenCV2
                    image = bridge.imgmsg_to_cv2(image_msg)
                except CvBridgeError as e:
                    print(e)
                    return


            if self._stop: return None  # Check for a request to stop.
            self.update_status_msg(image_msg.header)
            wx.CallAfter(self._parent.update_raw_image, image)
            wx.CallAfter(self._parent.update_remote_homography, homography)

        except rospy.service.ServiceException as e:
            rospy.logwarn('Service failed: {}'.format(e))
            return

        except wx._core.PyDeadObjectError:
            return

    def update_status_msg(self, img_header):
        # type: (std_msgs.msg.Header) -> unicode
        """Update the status bar for an image view"""
        t = img_header.stamp.to_sec()
        tnow = datetime.datetime.utcnow()
        t = datetime.datetime.utcfromtimestamp(t)
        diff = t - tnow
        extra_msg = ''
        # display a message if the time seems way off
        if diff.seconds > 5:
            extra_msg = ' [local UTC: ' + str(tnow) + '] = ' + str(diff)
        string = str(t) + extra_msg
        wx.CallAfter(self._parent.update_status_msg, string)
        return string

    def stop(self):
        self._stop = True


class UpdateImageThreadFit(UpdateImageThread):
    """Request imagery to be fit to the panel.

    """
    def __init__(self, parent, srv_topic, metadata_srv_topic,
                 compressed=False):
        """
        :param srv_topic: ROS topic for RequestImageView service to provide
            the required imagery.
        :type srv_topic: str

        :param metadata_srv_topic: ROS topic for RequestImageMetadata
            service to provide metadata about the imagery associated with
            srv_topic.
        :type metadata_srv_topic: str

        """
        # Initialize parent class
        super(UpdateImageThreadFit, self).__init__(parent, srv_topic,
                                                   metadata_srv_topic,
                                                   compressed=compressed)

    def get_homography(self):
        """Return homography to warp from panel to raw-image coordinates.

        """
        panel_width, panel_height = self._parent.wx_panel.GetSize()

        # Clamp to maximum requested size.
        s = MAX_MPIX/(panel_width*panel_height)
        if s < 1:
            s = np.sqrt(s)
            panel_height = int(np.round(panel_height*s))
            panel_width = int(np.round(panel_width*s))

        im_height = self._raw_image_height
        im_width = self._raw_image_width
        s = max(panel_height/im_height, panel_width/im_width)
        homography = np.array([[s,0,0],[0,s,0],[0,0,1]])

        output_height = np.floor(im_height*s)
        output_width = np.floor(im_width*s)
        return homography, output_height, output_width


class UpdateImageThreadZoom(UpdateImageThread):
    """Request imagery to be fit to the panel.

    """
    def __init__(self, parent, srv_topic, metadata_srv_topic, center_callback,
                 zoom_callback, compressed=False):
        """
        :param srv_topic: ROS topic for RequestImageView service to provide
            the imagery needed.
        :type srv_topic: str

        :param metadata_srv_topic: ROS topic for RequestImageMetadata
            service to provide metadata about the imagery associated with
            srv_topic.
        :type metadata_srv_topic: str

        """
        # Initialize parent class
        super(UpdateImageThreadZoom, self).__init__(parent, srv_topic,
                                                    metadata_srv_topic,
                                                    compressed=compressed)
        self.center_callback = center_callback
        self.zoom_callback = zoom_callback

    def _get_homography(self):
        """This is a stopgap to prevent this thread from spamming requests"""
        homography, panel_height, panel_width = self._get_homography()
        return homography, panel_height, panel_width

    def get_homography(self):
        """Return homography to warp from panel to raw-image coordinates.

        """
        #print('on_size')
        panel_width, panel_height = self._parent.wx_panel.GetSize()

        # Clamp to maximum requested size.
        s = MAX_MPIX/(panel_width*panel_height)
        if s < 1:
            s = np.sqrt(s)
            panel_height = int(np.round(panel_height*s))
            panel_width = int(np.round(panel_width*s))

        s = self.zoom_callback()/100
        center = self.center_callback()

        if center is None:
            if self._raw_image_height is not None and \
               self._raw_image_width is not None:
                   center = (self._raw_image_height/2,self._raw_image_width/2)

        #s = 1
        #center = (500,500)
        tx = panel_width/2-s*center[0]
        ty = panel_height/2-s*center[1]
        homography = np.array([[s,0,tx],[0,s,ty],[0,0,1]])
        return homography, panel_height, panel_width


class RemoteImagePanel(object):
    """Provide image updates to a wx.Panel using a remote imagery request.

    Attributes:
    homography - the homography applied to the image returned from
        UpdateImageThread so that it fits inside the current panel. Generally,
        the resolution of the image requested from the remote server will be
        chosen to match the panel resolution so that resizing is not required.
        However, if the panel is resized before a new request can be made and
        the updated image received, the image is resized using this homography
        for so that it still fits in the panels. Also, if the panel size
        exceeds the resolution limits set by 'MAX_MPIX', this homography will
        be used to enlarge the image received from the remote server to fit the
        panel.
    remote_homography -
    """
    def __init__(self, wx_panel, srv_topic, metadata_srv_topic,
                 status_static_text=None, compressed=False,
                 wx_histogram_panel=None):
        """
        :param wx_panel: Panel to add the image to.
        :type wx_panel: wx.Panel

        """
        self.wx_panel = wx_panel
        self.raw_image = None
        self.image = None
        self.raw_image_height = None
        self.raw_image_width = None
        self.panel_image_height = None
        self.panel_image_width = None
        self.homography = None
        self.remote_homography = None
        self.inverse_homography = None
        self.inverse_remote_homography = None
        self.wx_bitmap = None
        self.wx_histogram_panel = wx_histogram_panel
        self._histogram = None
        self._stop = False
        self.compressed = compressed
        self.last_update = None
        self.needs_update = True
        self.update_image_thread = None # type: UpdateImageThread
        self.run_once = True

        # Lock on access to self.raw_image.
        self.raw_image_lock = threading.RLock()

        #self.wx_panel.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)

        if wx_histogram_panel is not None:
            wx_histogram_panel.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)

        if wx_histogram_panel is not None:
            wx_histogram_panel.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)

        self.status_static_text = status_static_text
        self.srv_topic = srv_topic
        self.metadata_srv_topic = metadata_srv_topic

        self.start_image_thread()

        # ------------------------ Bind Events -------------------------------
        self.wx_panel.Bind(wx.EVT_LEFT_DOWN, self.on_click)
        self.wx_panel.Bind(wx.EVT_RIGHT_DOWN, self.on_click)
        self.wx_panel.Bind(wx.EVT_PAINT, self.on_paint_main)
        self.wx_panel.Bind(wx.EVT_SIZE, self.on_size)

        if wx_histogram_panel is not None:
            self.wx_histogram_panel.Bind(wx.EVT_PAINT, self.on_paint_histogram)
            self.wx_histogram_panel.Bind(wx.EVT_SIZE, self.on_size)
        # --------------------------------------------------------------------

    def start_image_thread(self):
        raise NotImplementedError('Subclass must define this method')

    def update_remote_homography(self, remote_homography):
        """Called by thread after providing an updated image.

        """
        self.remote_homography = remote_homography
        self.inverse_remote_homography = np.linalg.inv(remote_homography)

    def update_raw_image(self, raw_image):
        """Replace raw_image and update the rendered view in the panel.

        """
        if self._stop: return None  # Check for a request to stop.
        with self.raw_image_lock:
            self.raw_image = raw_image

            if self.wx_histogram_panel is not None:
                self.generate_histogram()

            self.update_all()
            self.last_update = time.time()

    def on_size(self, event):
        """Called on event wx.EVT_SIZE.

        """
        self.update_all()

    def update_all_if_needed(self):
        if self._stop or self.needs_update is False: return None  # Check for a request to stop.
        self.needs_update = False

        with self.raw_image_lock:
            if self.raw_image is not None:
                #print('on_size')
                panel_width, panel_height = self.wx_panel.GetSize()
                self.wx_image = wx.EmptyImage(panel_width, panel_height)
                self.update_homography()
                self.update_inverse_homography()
                self.warp_image()
                self.wx_panel.Refresh(True)

                if self.run_once:
                    self.wx_panel.GetParent().Layout()
                    self.run_once = False

                if self.wx_histogram_panel is not None:
                    self.wx_histogram_panel.Refresh(True)
            else:
                self.wx_bitmap = None
                self._histogram = None
                self.wx_panel.Refresh(True)

    def update_all(self):
        self.needs_update = True

    def update_homography(self):
        """Update homography mapping self.raw_image to the panel.

        """
        panel_width, panel_height = self.wx_panel.GetSize()
        im_height, im_width = self.raw_image.shape[:2]

        """
        if im_width/im_height > panel_width/panel_height:
            # Side edges of image should hit the edges of the panel.
            s = panel_width/im_width
            y = (panel_height-s*im_height)/2
            self.homography = np.array([[s,0,0],[0,s,y],[0,0,1]])
        else:
            # Top edges of image should hit the edges of the panel.
            s = panel_height/im_height
            x = (panel_width-s*im_width)/2
            self.homography = np.array([[s,0,x],[0,s,0],[0,0,1]])
        """
        s = min(panel_height/im_height, panel_width/im_width)
        self.homography = np.array([[s,0,0],[0,s,0],[0,0,1]])

        im_height, im_width = self.raw_image.shape[:2]
        corner_pts = np.array([[0,0,1],[0,im_height,1], [im_width,0,1],
                               [im_width,im_height,1]]).T
        corner_pts = np.dot(self.homography, corner_pts)
        corner_pts = corner_pts[:2]/corner_pts[2]
        self.panel_image_width = int(np.floor(max(corner_pts[0])))
        self.panel_image_height = int(np.floor(max(corner_pts[1])))

    def update_inverse_homography(self):
        """
        Calculate inverse of the homography.

        """
        if self.homography is None:
            return None
        else:
            self.inverse_homography = np.linalg.inv(self.homography)

    def warp_image(self):
        """Apply homography.

        """
        if self.raw_image is not None and self.inverse_homography is not None:
            panel_width, panel_height = self.wx_panel.GetSize()

            # Set linear interpolation.
            flags = cv2.INTER_LINEAR | cv2.WARP_INVERSE_MAP

            raw_image = self.raw_image

            image = cv2.warpPerspective(raw_image,
                                        self.inverse_homography,
                                        dsize=(self.panel_image_width,
                                               self.panel_image_height),
                                        flags=flags)

            if show_satured_pixels:
                if image.dtype == np.uint8:
                    maxval = 255
                elif image.dtype == np.uint16:
                    maxval = 65535

                if image.ndim == 2:
                    saturation_mask = image == 0
                    saturation_mask = image == maxval
                    saturation_mask = np.logical_or(image == maxval,
                                                    saturation_mask)
                else:
                    saturation_mask = np.all(image == 0, axis=-1)
                    saturation_mask = np.logical_or(np.all(image == maxval, -1),
                                                    saturation_mask)

            global contrast_strength
            image = stretch_image_contrast(image, contrast_strength)

            if raw_image.dtype == np.uint16:
                #np.save('/mnt/kamera/ir.npy', raw_image)
                raw_image = np.round(raw_image/256).astype('uint8')

            if image.ndim == 2:
                image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB)

            if show_satured_pixels:
                image[:, :, 0][saturation_mask] = 255
                image[:, :, 1][saturation_mask] = 0
                image[:, :, 2][saturation_mask] = 0

            wx_image = wx.EmptyImage(self.panel_image_width,
                                     self.panel_image_height)
            try:
                wx_image.SetData(image.tostring())
            except ValueError as err:
                raise ValueError('Shape: {} \n {}'.format(image.shape, err))
            self.wx_bitmap = wx_image.ConvertToBitmap()
        else:
            self.wx_bitmap = None

    def on_click(self, event):
        """Called on events wx.EVT_RIGHT_DOWN or wx.EVT_LEFT_DOWN.

        """
        if self.raw_image is not None and \
           self.inverse_remote_homography is not None:
            pos = list(event.GetPosition())
            panel_width, panel_height = self.wx_panel.GetSize()
            pos[0] -= (panel_width - self.panel_image_width)//2
            pos[1] -= (panel_height - self.panel_image_height)//2

            pos = np.dot(self.inverse_homography, [pos[0],pos[1],1])
            pos = np.dot(self.inverse_remote_homography, pos)
            pos = pos[:2]/pos[2]

            if event.LeftDown():
                button = 0
            elif event.RightDown():
                button = 1
            else:
                button = None

            self.process_clicked_point(pos, button)

    def process_clicked_point(self, pos, button):
        """
        :param pos: Raw image coordinates that were clicked.
        :type pos: 2-array

        :param button: The mouse button that was clicked (0 for left, 1 for
            right)
        :type button: 0 | 1

        """
        pass

    def generate_histogram(self):
        """Generate histogram for current image.

        """
        panel_width, panel_height = self.wx_histogram_panel.GetSize()

        if self.raw_image.dtype == np.uint8:
            v = np.histogram(self.raw_image, np.linspace(-0.5, 255.5, 257))[0]
        elif self.raw_image.dtype == np.uint16:
            v = np.histogram(self.raw_image, np.linspace(0, 65535, 257))[0]
        else:
            raise Exception(self.raw_image.dtype)

        vmax2, vmax1 = np.sort(v)[-2:]
        #vmax = min([vmax1,vmax2*2])
        vmax = vmax1
        v = v/(vmax)*panel_height
        h = panel_height - np.round(v).astype(np.int)
        image = np.full((panel_height,256,3), 255, np.uint8)

        for i in range(len(h)):
            image[h[i]:,i,1:] = 0

        self._histogram = image

    def on_paint_main(self, event=None):
        """Called on event wx.EVT_PAINT.

        """
        if self.wx_bitmap is not None:
            pdc = wx.PaintDC(self.wx_panel)
            dc = wx.GCDC(pdc)

            panel_width, panel_height = self.wx_panel.GetSize()
            dx = (panel_width - self.panel_image_width)//2
            dy = (panel_height - self.panel_image_height)//2
            dc.DrawBitmap(self.wx_bitmap, dx, dy)

        if event is not None:
            event.Skip()

    def on_paint_histogram(self, event=None):
        """Called on event wx.EVT_PAINT.

        """
        if self._histogram is not None and self.wx_histogram_panel is not None:
            panel_width, panel_height = self.wx_histogram_panel.GetSize()
            image = cv2.resize(self._histogram, dsize=(panel_width,
                               panel_height), interpolation=cv2.INTER_NEAREST)

            if True:
                wx_image = wx.EmptyImage(panel_width, panel_height)
                wx_image.SetData(image.tostring())
                wx_histogram_bitmap = wx_image.ConvertToBitmap()

                pdc = wx.PaintDC(self.wx_histogram_panel)
                dc = wx.GCDC(pdc)
                dc.DrawBitmap(wx_histogram_bitmap, 0, 0)

        if event is not None:
            event.Skip()

    def refresh(self, event):
        """Useful to bind the Refresh of self.wx_panel to an event.

        """
        event.Skip()
        self.wx_panel.Refresh(True)
        self.wx_histogram_panel.Refresh(True)

    def update_status_msg(self, string):
        if self.status_static_text is None: return None
        if self._stop: return None  # Check for a request to stop.
        string0 = self.status_static_text.GetLabel()
        self.status_static_text.SetLabel(string)
        self.status_static_text.SetForegroundColour((0,0,0))

        if len(string0) != len(string):
            self.status_static_text.GetParent().Layout()

    def release(self):
        # Set the _stop flag so that any threads can check and stop before
        # trying to access.
        self._stop = True

        self.wx_panel.Unbind(wx.EVT_PAINT)
        self.wx_panel.Unbind(wx.EVT_SIZE)

        self.update_image_thread.stop()


class RemoteImagePanelFit(RemoteImagePanel):
    """RemoteImagePanel with imagery fit to the size of the panel.

    """
    def __init__(self, wx_panel, srv_topic, metadata_srv_topic,
                 click_callback=None, status_static_text=None,
                 compressed=False, wx_histogram_panel=None):
        """
        :param wx_panel: Panel to add the image to.
        :type wx_panel: wx.Panel

        """
        super(RemoteImagePanelFit, self).__init__(wx_panel, srv_topic,
                                                  metadata_srv_topic,
                                                  status_static_text,
                                                  compressed,
                                                  wx_histogram_panel)
        self.click_callback = click_callback

    def start_image_thread(self):
        update_image_thread = UpdateImageThreadFit(self, self.srv_topic,
                                                   self.metadata_srv_topic,
                                                   compressed=self.compressed)
        update_image_thread.start()
        self.update_image_thread = update_image_thread

    def process_clicked_point(self, pos, button):
        """
        :param pos: Raw image coordinates that were clicked.
        :type pos: 2-array

        :param button: The mouse button that was clicked (0 for left, 1 for
            right)
        :type button: 0 | 1

        """
        if self.click_callback is not None:
            self.click_callback(pos, button)


class RemoteImagePanelZoom(RemoteImagePanel):
    """RemoteImagePanel with imagery zoomed to particular region.

    """
    def __init__(self, wx_panel, srv_topic, metadata_srv_topic, zoom_slider,
                 status_static_text=None, compressed=False,
                 wx_histogram_panel=None):
        """
        :param wx_panel: Panel to add the image to.
        :type wx_panel: wx.Panel

        """
        self._zoom = 100
        self._center = None
        super(RemoteImagePanelZoom, self).__init__(wx_panel,
                                                   srv_topic,
                                                   metadata_srv_topic,
                                                   status_static_text,
                                                   compressed,
                                                   wx_histogram_panel)

        self.zoom_slider = zoom_slider
        self.wx_panel.Bind(wx.EVT_MOUSEWHEEL, self.on_zoom_mouse_wheel)
        self.zoom_slider.Bind(wx.EVT_SCROLL, self.on_zoom_slider)

        # Use the default setting on the zoom slider to set initial zoom.
        self.on_zoom_slider()

    def start_image_thread(self):
        update_image_thread = UpdateImageThreadZoom(self, self.srv_topic,
                                                    self.metadata_srv_topic,
                                                    self.get_center,
                                                    self.get_zoom,
                                                    compressed=False)
        update_image_thread.start()
        self.update_image_thread = update_image_thread

    def get_zoom(self):
        """Zoom percentage.

        """
        return self._zoom

    def set_zoom(self, zoom):
        """
        :param zoom: Zoom percentage.
        :type zoom: float
        """
        self._zoom = zoom
        self.update_all()

    def get_center(self):
        """Zoom percentage.

        """
        return self._center

    def set_center(self, center):
        self._center = center
        self.update_all()

    def process_clicked_point(self, pos, button):
        """
        :param pos: Raw image coordinates that were clicked.
        :type pos: 2-array

        :param button: The mouse button that was clicked (0 for left, 1 for
            right)
        :type button: 0 | 1

        """
        self.set_center(pos)

    def on_zoom_mouse_wheel(self, event=None):
        val = event.GetWheelRotation()
        if event.ShiftDown():
            change = 1.1
        else:
            change = 1.01

        if val > 0:
            self.on_zoom_up(change=change)
        if val < 0:
            self.on_zoom_down(change=change)

    def on_zoom_up(self, event=None, change=1.02):
        zoom = np.minimum(self._zoom*change, 2000)
        self.handle_updated_zoom(zoom)

    def on_zoom_down(self, event=None, change=1.02):
        zoom = np.maximum(self._zoom/change, 10)
        self.handle_updated_zoom(zoom)

    def handle_updated_zoom(self, zoom):
        self.set_zoom(zoom)

    def on_zoom_slider(self, event=None):
        """Slider takes values from 0 to 1000.
        """
        v = self.zoom_slider.GetValue()
        v = v/1000.0
        self.set_zoom(2*(1-v) + (1000-2)*v)

        if event is not None:
            self.update_all()


class FullAndZoomView(object):
    def __init__(self, full_view_panel, zoomed_view_panel, histogram_panel,
                 zoom_slider, status_static_text, srv_topic,
                 metadata_srv_topic, compressed=False):
        """
        :param wx_panel: Panel to add the image to.
        :type wx_panel: wx.Panel

        """
        self.zoom_panel = RemoteImagePanelZoom(wx_panel=zoomed_view_panel,
                                               srv_topic=srv_topic,
                                               zoom_slider=zoom_slider,
                                               compressed=compressed,
                                               metadata_srv_topic=metadata_srv_topic)

        self.fit_panel = RemoteImagePanelFit(wx_panel=full_view_panel,
                                             srv_topic=srv_topic,
                                             metadata_srv_topic=metadata_srv_topic,
                                             click_callback=self.zoom_panel.process_clicked_point,
                                             status_static_text=status_static_text,
                                             compressed=compressed,
                                             wx_histogram_panel=histogram_panel)

    def set_zoom(self, zoom):
        """
        :param zoom: Zoom percentage.
        :type zoom: float
        """
        self._zoom = zoom
        self.zoom_label.SetLabel('{}%'.format(int(np.round(zoom))))
        self.zoom_panel.update_all()

    def set_center(self, center):
        """
        :param center: Location for the zoom center in the original image's coordinates.
        :type center: 2-array
        """
        self._center = center
        self.zoom_panel.update_all()

    def release(self):
        self.fit_panel.release()
        self.zoom_panel.release()


class MainFrame(form_builder_output.MainFrame):
    #constructor
    def __init__(self, node_name, topic_names, compressed=False,
                 window_title='System Control Panel'):
        """
        :param node_name:
        :type node_name: str

        :param topic_names: Dictionary of topic names.
        :type topic_names: dict

        """
        #initialize parent class
        form_builder_output.MainFrame.__init__(self, None)
        self.SetTitle(window_title)
        icon = wx.EmptyIcon()
        icon.CopyFromBitmap(wx.Bitmap("%s/config/seal-icon.png" % PKG_DIR, wx.BITMAP_TYPE_ANY))
        self.SetIcon(icon)

        # Initialize other frames.
        self._hot_key_list = False
        self._camera_config_frame = False
        self._image_inspection_frame = False
        self._base_layer_frame = False
        self._system_control_frame = False
        self._collect_in_region = None
        self._last_collect_in_region_check_time = -np.inf
        self._last_fixed_overlap_check_time = -np.inf
        self._last_fixed_overlap_error_time = -np.inf
        self._base_fpath = "/mnt/flight_data"

        self.topic_names = topic_names
        self.last_popup = datetime.datetime.now()
        self.last_msg_txt = ''

        # Setting wx.CB_READONLY doesn't allow you to SetEditable(True) later.
        self.effort_combo_box.SetEditable(False)

        self._collecting = None
        self.collecting = rospy.get_param("/sys/arch/is_archiving", False)    # Triggers a GUI update.

        self._camera_configuration_dict = {}

        self._claheClipLimit = 200

        self.load_config_settings()

        if False:
            try:
                self.load_camera_config_dict()
            except Exception as e:
                rospy.logerr('Caught error: {}: {}'.format(type(e).__name__, e))

        self.update_show_hide()

        # Set up ROS connections.
        rospy.init_node(node_name, anonymous=False)

        # --------------------------- Image Streams --------------------------
        # These will all be compressed images fit to the panel.
        self.remote_image_panels = []

        # RGB images.
        image_topic = topic_names['rgb_image_topic']
        ret = RemoteImagePanelFit(self.camera1_panel,
                                  '%s/view_service' % image_topic,
                                  '%s/metadata_service' % image_topic,
                                  None, self.camera1_status_text,
                                  compressed, self.camera1_histogram_panel)
        self.remote_image_panels.append(ret)

        image_topic = topic_names['rgb_analytics_image_topic']
        ret = RemoteImagePanelFit(self.camera1_analytics_panel,
                                  '%s/view_service' % image_topic,
                                  '%s/metadata_service' % image_topic,
                                  None, self.camera1_analytics_status_text,
                                  compressed, None)
        self.remote_image_panels.append(ret)
        # --------------------------------------------------------------------

        # ------------------------------ Cameras -----------------------------
        self.rgb_genicam_controller = RCGenicamCameraController(self,
                                                                topic_names['rgb_genicam_get_service_topic'],
                                                                topic_names['rgb_genicam_set_service_topic'])
        # --------------------------------------------------------------------

        # ----------------------------- INS ----------------------------------
        self.lat0 = None
        self.lon0 = None
        self.h0 = None
        self.geod_offset = None

        self.spatial_ins_interface = SpatialINSInterface(
                                        topic_names['nav_sat_fix_topic'],
                                        topic_names['imu_fix_topic'],
                                        topic_names['nav_filter_status_topic'],
                                        self.ins_update)
        # --------------------------------------------------------------------


        # ----------------------------- Computer -----------------------------
        self.diagnostic_status_subs = []
        sub = rospy.Subscriber(topic_names['ptp_health_ins'], DiagnosticStatus,
                               lambda msg: self.on_diagnostic_status_ros(msg, self.ptp_ins_status_txt),
                               queue_size=1)
        self.diagnostic_status_subs.append(sub)

        sub = rospy.Subscriber(topic_names['ptp_health_nmea'], DiagnosticStatus,
                               lambda msg: self.on_diagnostic_status_ros(msg, self.ptp_nmea_status_txt),
                               queue_size=1)
        self.diagnostic_status_subs.append(sub)

        sub = rospy.Subscriber(topic_names['ptp_health_pps'], DiagnosticStatus,
                               lambda msg: self.on_diagnostic_status_ros(msg, self.ptp_1pps_status_txt),
                               queue_size=1)
        self.diagnostic_status_subs.append(sub)

        sub = rospy.Subscriber(topic_names['ptp_health_ptp'], DiagnosticStatus,
                               lambda msg: self.on_diagnostic_status_ros(msg, self.ptp_status_txt),
                               queue_size=1)
        self.diagnostic_status_subs.append(sub)
        # --------------------------------------------------------------------


        # ---------------------------- Archiving -----------------------------
        # --------------------------------------------------------------------


        # ----------------------------- Hot Keys -----------------------------
        if False:
            entries = []

            # Bind ctrl+s to start/stop recording.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.reverse_collecting_state, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('S'), random_id)

            # Bind ctrl+d to start detectors.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.on_start_detectors, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('D'), random_id)

            # Bind ctrl+f to stop detectors.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.on_stop_detectors, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('F'), random_id)

            # Bind ctrl+h to hot key menu.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.on_hot_key_help, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('H'), random_id)

            # Bind ctrl+e to set context to exposure entry.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.set_focus_to_exposure, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('E'), random_id)

            # Bind ctrl+n to add note to log.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.on_add_to_event_log, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('N'), random_id)

            # Bind ctrl+o to next previous camera configuration.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.previous_camera_config, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('O'), random_id)

            # Bind ctrl+p to next camera configuration.
            entries.append(wx.AcceleratorEntry())
            random_id = wx.NewId()
            self.Bind(wx.EVT_MENU, self.next_camera_config, id=random_id)
            entries[-1].Set(wx.ACCEL_CTRL, ord('P'), random_id)

            accel = wx.AcceleratorTable(entries)
            self.SetAcceleratorTable(accel)

            self.exposure_value_txt_ctrl.Bind(wx.EVT_KEY_DOWN, self.on_exp_key_press)
        # --------------------------------------------------------------------

        #rospy.add_client_shutdown_hook(self.on_close_button)

        self.Bind(wx.EVT_CLOSE, self.when_closed)
        self.Bind(wx.EVT_SIZE, self.on_resize)
        self.effort_combo_box.Bind( wx.EVT_COMBOBOX, self.save_config_settings)

        # So that we can check that the node is still alive.
        self.timer = wx.Timer(self)
        self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
        FAST_TIMER_MS = 1/GUI_UPDATE_THROTTLE*1000
        self.timer.Start(FAST_TIMER_MS)

        # So that we can check that the node is still alive.
        self.slow_timer = wx.Timer(self)
        self.Bind(wx.EVT_TIMER, self.on_slow_timer, self.slow_timer)
        self.slow_timer.Start(1000)
        self.delay = 0

        self.Show()
        #print(self.GetSize())
        #self.SetMinSize((1117, 996))
        if self.collecting:
            self._disable_state_controls()

    def on_exp_key_press(self, event):
        # type: (wx.KeyEvent) -> None
        """13 is enter. wx.stc.STC_KEY_RETURN must be a later version or something"""
        if event.GetKeyCode() in [13, 370]:
            self.on_set_exposure(event)
        else:
            event.Skip()

    def on_slow_timer(self, event):
        diskspace_free = 0
        self.disk_usage_static_text.SetLabel("Disk Space: %0.2f GB" %
                                             diskspace_free)
        self.delay += 1

    def on_timer(self, event):
        """Manages all updates that should happen at fixed rate.

        """
        tic = time.time()
        if rospy.is_shutdown():
            self.on_close_button(None)

        self.update_static_text()

        # Check to see if imagery has been received recently.
        for panel in self.remote_image_panels:
            # Refresh images if needed
            panel.update_all_if_needed()
            if panel.last_update is None:
                panel.status_static_text.SetLabel('No Imagery')
                panel.status_static_text.SetForegroundColour(BRIGHT_RED)
            else:
                dt = time.time() - panel.last_update
                if dt > 5:
                    panel.status_static_text.SetLabel('No update in %0.3f s' % dt)
                    panel.status_static_text.SetForegroundColour(BRIGHT_RED)

        # Check to see if imagery has been received recently.
        if self._image_inspection_frame:
            zoom = self._image_inspection_frame.full_view_rp.zoom_panel
            fit = self._image_inspection_frame.full_view_rp.fit_panel
            # Refresh images if needed
            if fit is not None:
                fit.update_all_if_needed()

            if zoom is not None:
                zoom.update_all_if_needed()

        if self.gui_config_dict['collection_mode'] == 'fixed overlap':
            if self._last_fixed_overlap_check_time + 1 < time.time():
                # Calculate frame rate such that we achieve a fixed overlap.
                self._last_fixed_overlap_check_time = time.time()
                try:
                    overlap = self.gui_config_dict['collection_mode_parameter']
                    overlap = overlap/100
                    fov = self.gui_config_dict['rgb_vfov']
                    speed = self.speed_si  #float(self.speed_txt_ctrl.GetValue())
                    alt = float(self.alt_msl_txt_ctrl.GetValue())

                    # Width of the field of view in the flight direction.
                    w = np.tan(fov/180*np.pi/2)*alt*2

                    # Distance on ground covered before next acquisition.
                    w = w*(1-overlap)
                    rate = speed/w
                    #print(fov, speed, alt, w, rate)

		    if rate > G_MAX_FRAME_RATE:
                        raise ValueError("Fixed overlap for given speed/alt tries to set capture rate to {:.3f} Hz, higher "
                                         "than maximum {} Hz. Please decrease overlap or speed".format(rate, G_MAX_FRAME_RATE))

                except ValueError as exc:
                    if self._last_fixed_overlap_error_time + 30 < time.time():
                        self._last_fixed_overlap_error_time = np.inf
                        msg = str(exc)
                        dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
                        dlg.ShowModal()
                        dlg.Destroy()
                        self.on_set_collection_mode()
                        self._last_fixed_overlap_error_time = time.time()

                    else:
                        return
                except Exception as e:
                    rospy.logerr('Caught error: {}: {}'.format(type(e).__name__, e))
                    return

                self.send_frame_rate_update(rate)
        #print("Time for sync loop %0.3fs" % (time.time() - tic))

    def on_resize(self, event):
        event.Skip()

    # -------------------------------- Properties -----------------------------
    @property
    def effort_metadata_dict(self):
        # type: () -> dict
        return self.gui_config_dict['effort_metadata_dict']

    @property
    def camera_configuration_dict(self):
        # type: () -> dict
        return self._camera_configuration_dict

    @property
    def flight_number_str(self):
        # type: () -> str
        return self.gui_config_dict.get('flight_number_str', 'no_flight_num')

    @flight_number_str.setter
    def flight_number_str(self, val):
        self.gui_config_dict['flight_number_str'] = val
        self.save_config_settings()

    @property
    def collecting(self):
        """Specifies whether data is currently being collected.

        """
        return self._collecting

    @collecting.setter
    def collecting(self, val):
        # type: (bool) -> None
        self._collecting = val

        self.update_collect_colors(val, self._collect_in_region)

    def update_collect_colors(self, is_collecting=None, collect_in_region=None):
        is_collecting = self._collecting if is_collecting is None else is_collecting
        collect_in_region = self._collect_in_region if collect_in_region is None else collect_in_region
        if is_collecting == True:
            self.recording_gauge.SetBackgroundColour((0, 255, 0))
            self.flight_data_panel.SetBackgroundColour(COLLECT_GREEN)

        elif is_collecting == False:
            self.recording_gauge.SetBackgroundColour((200, 200, 200))
            if collect_in_region is None:
                self.flight_data_panel.SetBackgroundColour(APP_GRAY)
            else:
                self.flight_data_panel.SetBackgroundColour(SHAPE_COLLECT_BLUE)
        else:
            raise Exception("invalid value encountered for is_collecting")

    @property
    def collect_in_region(self):
        # type: () -> Optional[shapely.geometry.base.BaseGeometry]
        return self._collect_in_region
    # ------------------------------------------------------------------------

    def load_config_settings(self):
        with open(config_filename, "r") as input_file:
            self.gui_config_dict = json.load(input_file)

        print('Loaded config: \n{}'.format(json.dumps(self.gui_config_dict, indent=2)))

        self.flight_number_text_ctrl.Unbind(wx.EVT_TEXT)
        self.flight_number_text_ctrl.SetValue(self.flight_number_str)
        self.flight_number_text_ctrl.Bind(wx.EVT_TEXT,
                                          self.on_update_flight_number)

        global contrast_strength
        contrast_strength = self.gui_config_dict['contrast_strength']
        if self.gui_config_dict.get('shapefile_fname', None):
            self.set_collect_in_region(self.gui_config_dict.get('shapefile_fname'))
        self.update_collect_colors()

    def save_config_settings(self, event=None):
        self.gui_config_dict['effort_selection'] = self.effort_combo_box.GetSelection()

        global contrast_strength
        self.gui_config_dict['contrast_strength'] = contrast_strength

        with open(config_filename, "w") as output_file:
            json.dump(self.gui_config_dict, output_file)

        if event is not None:
            event.Skip()

    def update_static_text(self):
        pass

    def on_set_exposure(self, event=None):
        """Called when 'Set Parameter' button is pressed (formerly Set Exposure).
        Exposure message in the driver expects int32 in microseconds
        """
        fov = self.camera_setting_subsys.GetString(
            self.camera_setting_subsys.GetCurrentSelection()).lower()

        chan = self.camera_setting_rgb_uv_combo.GetString(
            self.camera_setting_rgb_uv_combo.GetCurrentSelection()).lower()
        param = self.camera_setting_exposure_gain.GetString(
            self.camera_setting_exposure_gain.GetCurrentSelection()).lower()
        value_str = self.exposure_value_txt_ctrl.GetValue()
        factor = 1e-3

        try:
            # gui is in millis, log is in millis, API call is in microseconds
            value_float = float(value_str)

        except ValueError:
            msg = 'Settings value must be a valid number'
            dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
            dlg.ShowModal()
            dlg.Destroy()
            return

        if 'exposure' in param:
            cam_param = 'ExposureValue'
            if value_float > G_MAX_EXPOSURE_MS:
                msg = 'Exposure must be less than {} ms. High exposure may cause instability'.format(G_MAX_EXPOSURE_MS)
                dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
                dlg.ShowModal()
                dlg.Destroy()
                return
            value_float *= 1000 # ms to microseconds
        elif 'gain' in param:
            cam_param = 'GainValue'
        else:
            raise RuntimeError('unexpected  param: {}'.format(param))

        if fov == 'all':
            targets = FOV2IDX.keys()
        else:
            targets = [fov]

        print('TARGETS: {}'.format(targets))
        func = partial(self.set_cam_attr, chan=chan, param=cam_param, val=value_float)
        map(func, targets)

    def send_frame_rate_update(self, rate):
        """Send an updated triggering rate for camera on remote systems.
            NOTE: trigger control moved to redis
        """
        msg = std_msgs.msg.Float64()
        rate = np.clip(rate, 0.1, G_MAX_FRAME_RATE)
        msg.data = rate
        # self.pub_trigger_freq.publish(msg) ## deprecated but leaving this line for easy comment testing
        kv.put('/sys/arch/trigger_freq', json.dumps(float(rate)))

    def start_collecting(self, event=None):
        service = rospy.ServiceProxy(
            '/recorder/start_recording', start_recording, persistent=False)
        service()
        self._disable_state_controls()
        self.log_state()

        #missed_frame_store.reset_count()
        effort_name = self.effort_combo_box.GetStringSelection()

        observer = self.observer_text_ctrl.GetValue()
        if observer != '':
            self.add_to_event_log('Observer: %s' % observer)

        wx.CallAfter(self._set_archving, *(1, 0))
        wx.CallAfter(self._set_archving, *(1, 1))
        wx.CallAfter(self._set_archving, *(1, 2))
        collecting = True
        self.collecting = collecting
        self.update_project_flight_params(collecting=collecting)
        self.add_to_event_log('Started collecting for effort: %s' % effort_name)

    def stop_collecting(self, event=None):
        service = rospy.ServiceProxy(
            '/recorder/stop_recording', stop_recording, persistent=False)
        service()
        self._enable_state_controls()
        effort_name = self.effort_combo_box.GetStringSelection()

        wx.CallAfter(self._set_archving, *(0, 0))
        wx.CallAfter(self._set_archving, *(0, 1))
        wx.CallAfter(self._set_archving, *(0, 2))
        collecting = False
        self.collecting = collecting
        self.update_project_flight_params(collecting=collecting)
        self.add_to_event_log('Stopped collecting for effort: %s' % effort_name)

    def reboot_system(self, event=None):
        print('reboot sent')
        service = rospy.ServiceProxy(
            '/recorder/reboot', reboot, persistent=False)
        service()

    def shutdown_system(self, event=None):
        print('shutdown sent')
        service = rospy.ServiceProxy(
            '/recorder/shutdown', shutdown, persistent=False)
        service()

    def _set_archving(self, archiving, sys_ind):
        msg = 'blank'
        if sys_ind == 0:
            srv = self._center_sys_archive_srv
            system_name = 'Center-View'
        elif sys_ind == 1:
            srv = self._left_sys_archive_srv
            system_name = 'Left-View'
        elif sys_ind == 2:
            srv = self._right_sys_archive_srv
            system_name = 'Right-View'
        else:
            raise Exception('Invalid \'sys_ind\' %s' % str(sys_ind))

        self.update_project_flight_params()
        self.save_camera_config_dict(self.camera_configuration_dict)

        try:
            project, flight, effort_name, sys_cfg = self.get_project_flight()
            collection_mode = str(self.gui_config_dict['collection_mode'])

            msg = '\'%s\' system not responsive' % system_name
            state = {'is_archiving': archiving, 'project': project,
                     'effort': effort_name, 'flight': flight, 'sys_cfg':sys_cfg}

            resp = verbose_call_service(srv,
                                        archiving=archiving,
                                        project=project,
                                        effort=effort_name,
                                        flight=flight,
                                        collection_mode=collection_mode
                                        )
            if not resp:
                msg = '\'{}\' system responded with {}'.format(system_name, resp)
                raise Exception('Returned {}'.format(resp))
        except Exception as e:
            state = None
            if archiving:
                dlg = wx.MessageDialog(self, msg, 'Error: {}'.format(e),
                                       wx.OK | wx.ICON_ERROR)
                dlg.ShowModal()
                dlg.Destroy()
            return
        if state is not None:
            kv.put("/sys/arch", state)

    def exception_window(self, e, msg='An error occured'):
        dlg = wx.MessageDialog(self, 'Error: {}'.format(e), msg,
                               wx.OK | wx.ICON_ERROR)
        dlg.ShowModal()
        dlg.Destroy()

    # ---------------------------- ROS Callbacks -----------------------------
    def on_diagnostic_status_ros(self, msg, wx_status_txt):
        wx.CallAfter(self.on_diagnostic_ptp, msg, wx_status_txt)

    def on_diagnostic_ptp(self, msg, wx_status_txt):
        set_diagnostic_level_symbol(wx_status_txt, msg.level)

    # -------------------- Launch Post Proc Scripts --------------------------

    def on_detection_summary(self, event):
        dialog = wx.DirDialog(None, "Choose a Flight Directory",
                              '/mnt/flight_data')
        if dialog.ShowModal() == wx.ID_OK:
            flight_dir = dialog.GetPath()
        else:
            print("Invalid path selected, not setting.")
            return
        system.run_command("postproc", "nuvo0", "detections", d=flight_dir)
        self.add_to_event_log('command sent: Create detection summary for flight {}. '.format(
                                flight_dir))

    def on_measure_image_to_image_homographies(self, event):
        dialog = wx.DirDialog(None, "Choose a Flight Directory",
                              '/mnt/flight_data')
        if dialog.ShowModal() == wx.ID_OK:
            flight_dir = dialog.GetPath()
        else:
            print("Invalid path selected, not setting.")
            return
        system.run_command("postproc", "nuvo0", "homography", d=flight_dir)
        self.add_to_event_log('command sent: Fine tune tracking for flight {}. '.format(
                                flight_dir))

    def on_create_flight_summary(self, event):
        dialog = wx.DirDialog(None, "Choose a Flight Directory",
                              '/mnt/flight_data')
        if dialog.ShowModal() == wx.ID_OK:
            flight_dir = dialog.GetPath()
        else:
            print("Invalid path selected, not setting.")
            return
        system.run_command("postproc", "nuvo0", "summary", d=flight_dir)
        self.add_to_event_log('command sent: Create flight summary for flight {}. '.format(
                                flight_dir))

    # ------------------------------------------------------------------------

    # -------------------- Launch Image Inspection Frame ---------------------
    def on_dclick_camera1(self, event):
        self.open_image_inspection_panel('Camera 1')

    def on_dclick_camera2(self, event):
        self.open_image_inspection_panel('Camera 2')
    # ------------------------------------------------------------------------

    def cb_raw_message_popup(self, msg):
        wx.CallAfter(self.raw_message_popup, msg)

    def raw_message_popup(self, msg):
        print("/rawmsg: \n{}".format(msg))
        return self.message_popup_throttle(msg.data, throttle=10)

    def message_popup_throttle(self, txt, throttle=None):
        now = datetime.datetime.now()
        if throttle is None:
            pass
        else:
            td_throttle = datetime.timedelta(seconds=throttle)
            dt = now - self.last_popup
            if dt < td_throttle:
                rospy.logwarn('suppressed, dt too short: {} : {}'.format(dt, txt))
                return
        icon = wx.ICON_ERROR if 'error' in txt.lower() else wx.ICON_INFORMATION
        dlg = wx.MessageDialog(self, txt, 'Info', wx.OK | icon)
        dlg.ShowModal()
        dlg.Destroy()
        self.last_popup = now

    def ins_update(self, t=None, lat=None, lon=None, h=None, easting_std=None,
                   northing_std=None, h_std=None, total_speed=None,
                   heading=None, pitch=None, roll=None, heading_std=None,
                   pitch_std=None, roll_std=None, orientation_fiter_init=None,
                   navigation_filter_init=None, heading_initialized=None,
                   utc_time_initialized=None, internal_gnss_enabled=None,
                   magnetic_heading=None, velocity_heading=None,
                   atmospheric_altitude=None):
        """Pass INS-derived information to the GUI to be updated.

        :param t: Latest time reported by the INS in seconds.
        :type t: float

        :param lat: Latitude (degrees).
        :type lat: float

        :param lon: Longitude (degrees).
        :type lon: float

        :param h: Height above WGS94 ellipsoid.
        :type h: float

        :param easting_std: Position estimation easting standard deviation.
        :type easting_std: float

        :param northing_std: Position estimation northing standard deviation.
        :type northing_std: float

        :param h_std: Position estimation height standard deviation.
        :type h_std: float

        :param total_speed: Total speed (m/s).
        :type total_speed: float

        :param heading: INS heading (degrees).
        :type heading: float

        :param pitch: INS pitch (degrees).
        :type pitch: float

        :param roll: INS roll (degrees).
        :type roll: float

        """
        if lat is not None:
            self.lat_txt_ctrl.SetValue("{0:.8f}".format(lat))

        if lon is not None:
            self.lon_txt_ctrl.SetValue("{0:.8f}".format(lon))

        if h is not None:
            self.alt_txt_ctrl.SetValue("{0:.3f}".format(h))

        if easting_std is not None:
            self.easting_std_txt_ctrl.SetValue("{0:.2f}".format(easting_std))

        if northing_std is not None:
            self.northing_std_txt_ctrl.SetValue("{0:.2f}".format(northing_std))

        if h_std is not None:
            self.h_std_txt_ctrl.SetValue("{0:.2f}".format(h_std))

        # Location of geod (i.e., mean sea level, which is
        # generally the ground for us) relative to the
        # ellipsoid. Positive value means that mean sea level is
        # above the WGS84 ellipsoid.
        if lat is not None and lon is not None:
            self.geod_offset = geod.height(lat, lon)

        if h is not None and self.geod_offset is not None:
            self.alt_msl_txt_ctrl.SetValue("{0:.3f}".format(h - self.geod_offset))

        if total_speed is not None:
            speed_si = total_speed  # meters/second
            self.speed_si = speed_si
            speed_kts = speed_si * 1.94384  # knots
            self.speed_txt_ctrl.SetValue("{0:.3f}".format(speed_kts))

        if heading is not None:
            self.heading_txt_ctrl.SetValue("{0:.3f}".format(heading))

        if pitch is not None:
            self.pitch_txt_ctrl.SetValue("{0:.3f}".format(pitch))

        if roll is not None:
            self.roll_txt_ctrl.SetValue("{0:.3f}".format(roll))

        if heading_std is not None:
            self.heading_std_txt_ctrl.SetValue("{0:.2f}".format(heading_std))

        if pitch_std is not None:
            self.pitch_std_txt_ctrl.SetValue("{0:.2f}".format(pitch_std))

        if roll_std is not None:
            self.roll_std_txt_ctrl.SetValue("{0:.2f}".format(roll_std))

        if t is not None:
            t = str(datetime.datetime.utcfromtimestamp(np.round(t*100)/100))
            t = t[:22]
            self.ins_time_txt_ctrl.SetValue(t)

        # Update status for various INS elements. If the value is None, the
        # function call does nothing.
        velocity_heading = False

        if orientation_fiter_init is not None:
            set_diagnostic_level_symbol(self.orientation_filter_status_txt,
                                        0 if orientation_fiter_init else 2)

        if navigation_filter_init is not None:
            set_diagnostic_level_symbol(self.navigation_filter_status_txt,
                                        0 if navigation_filter_init else 2)

        if heading_initialized is not None:
            set_diagnostic_level_symbol(self.heading_initialized_status_txt,
                                        0 if heading_initialized else 2)

        if utc_time_initialized is not None:
            set_diagnostic_level_symbol(self.utc_time_status_txt,
                                        0 if utc_time_initialized else 2)

        if internal_gnss_enabled is not None:
            set_diagnostic_level_symbol(self.internal_gnss_status_txt,
                                        0 if internal_gnss_enabled else 2)

        if magnetic_heading is not None:
            set_diagnostic_level_symbol(self.magnetic_heading_status_txt,
                                        0 if magnetic_heading else 2)

        if velocity_heading is not None:
            set_diagnostic_level_symbol(self.velocity_heading_status_txt,
                                        0 if velocity_heading else 2)

        if atmospheric_altitude is not None:
            set_diagnostic_level_symbol(self.atmospheric_altitude_status_txt,
                                        0 if atmospheric_altitude else 2)

        if self.collect_in_region is not None:
            if self._last_collect_in_region_check_time + 1 < time.time():
                point = shapely.geometry.Point(lon, lat)
                if self.collect_in_region.contains(point):
                    if not self.collecting:
                        self.start_collecting()
                elif self.collecting:
                    self.stop_collecting()

                self._last_collect_in_region_check_time = time.time()

        if self._base_layer_frame:
            if (lat is not None and lon is not None and \
                easting_std is not None and northing_std is not None):
                self._base_layer_frame.set_ins_state(lat, lon,
                                                     np.diag([easting_std**2,
                                                              northing_std**2]))
            self._base_layer_frame.set_ins_state(roll=roll, pitch=pitch,
                                                 heading=heading)

    def on_update_flight_number(self, event=None):
        self.flight_number_str = self.flight_number_text_ctrl.GetValue()
        self.add_to_console_log(self.flight_number_str, 'on_update_flight_number')
        self.save_config_settings()


    # ---------------------------- Show/Hide Images --------------------------
    def on_show_or_hide_camera1(self, event):
        """Toggle visilibity of left column of images.

        """
        self.gui_config_dict['show_camera1'] = not self.gui_config_dict['show_camera1']
        self.save_config_settings()
        self.update_show_hide()

    def on_show_or_hide_camera2(self, event):
        """Toggle visilibity of center column of images.

        """
        self.gui_config_dict['show_camera2'] = not self.gui_config_dict['show_camera2']
        self.save_config_settings()
        self.update_show_hide()

    def update_show_hide(self):
        """Update which image panes are to be shown or hidden.

        """
        # RGB
        if self.gui_config_dict['show_camera1']:
            self.m_panel_camera1.Show()
        else:
            self.m_panel_camera1.Hide()

        if self.gui_config_dict['show_camera2']:
            self.m_panel_camera2.Show()
        else:
            self.m_panel_camera2.Hide()

        self.Layout()
    # ------------------------------------------------------------------------

    # ---------------------------- Detection Menu ----------------------------
    def do_start_a_detector(self, event=None, host='unset', log=True):
        set_detector_state(host, EPodStatus.Running)
        if log:
            self.add_to_event_log('command sent: start detector {}'.format(host))

    def do_stop_a_detector(self, event=None, host='unset', log=True):
        set_detector_state(host, EPodStatus.Off)
        if log:
            self.add_to_event_log('command sent: stop detector {}'.format(host))

    def on_start_detectors(self, event=None):
        self.add_to_event_log('command sent: start all detectors')
        hosts = ['nuvo0', 'nuvo1', 'nuvo2']
        for host in hosts:
            self.do_start_a_detector(host=host, log=False)

    # ------------------------- Methods for Hot Keys -------------------------
    def set_focus_to_exposure(self, event=None):
        self.exposure_value_txt_ctrl.SetFocus()

    def reverse_collecting_state(self, event=None):
        """If collecting, stop. If not collecting, start collecting.

        """
        if self.collecting:
            self.stop_collecting()
        else:
            self.start_collecting()
    # ------------------------------------------------------------------------

    def load_camera_model(self, name, fname):
        wildcard = "Camera model (*.yaml)"
        dialog = wx.FileDialog(None, "Choose a file", os.getcwd(), "",
                               wildcard, wx.OPEN)
        if dialog.ShowModal() == wx.ID_OK:
            print(dialog.GetPath())
    # ------------------------------------------------------------------------

    def save_flight_summary(self, event=None):
        wildcard = "Camera model (*.yaml)"
        dialog = wx.FileDialog(None, "Choose a file", '/mnt', "",
                               wildcard, wx.SAVE)
        if dialog.ShowModal() == wx.ID_OK:
            print(dialog.GetPath())

    # ----------------------------- Open New Panels -------------------------
    def open_image_inspection_panel(self, stream):
        if not self._image_inspection_frame:
            self._image_inspection_frame = ImageInspectionFrame(None,
                                                                self.topic_names,
                                                                self.save_config_settings,
                                                                stream,
                                                                compressed=False)

        self._image_inspection_frame.Raise()

    def on_base_layer_frame_raise(self, stream):
        if not self._base_layer_frame:
            self._base_layer_frame = BaseLayerFrame(None)

        self._base_layer_frame.Raise()

    def on_system_control_frame_raise(self, event=None):
        """Show list of hot keys.

        """
        if not self._system_control_frame:
            self._system_control_frame = SystemControlFrame(self,
                                                            [self.rgb_genicam_controller])

        self._system_control_frame.Raise()

    def on_set_collection_mode(self, event=None):
        """Triggered when 'Set Collection Mode' button is pressed.

        """
        if not self._collection_mode_frame:
            self._collection_mode_frame = CollectionModeFrame(self,
                                                              self.gui_config_dict['collection_mode'],
                                                              self.gui_config_dict['collection_mode_parameter'],
                                                              self.gui_config_dict.get('shapefile_fname', None),
                                                              self.set_collection_mode,
                                                              self.set_collect_in_region)

        self._collection_mode_frame.Raise()

    def set_collection_mode(self, mode, parameter):
        self.gui_config_dict['collection_mode'] = mode
        self.gui_config_dict['collection_mode_parameter'] = parameter

        self.add_to_event_log('Setting collection mode to \'%s\' with value %s'
                              % (mode,str(parameter)))

        self.save_config_settings()
        i_collecting = int(self.collecting)
        wx.CallAfter(self._set_archving, *(i_collecting, 0))
        wx.CallAfter(self._set_archving, *(i_collecting, 1))
        wx.CallAfter(self._set_archving, *(i_collecting, 2))

        if mode == 'fixed rate':
            self.send_frame_rate_update(parameter)

    def set_collect_in_region(self, fname):
        """Load shapefile 'fname' to be used for collection region trigger.

        """
        self.gui_config_dict['shapefile_fname'] = fname
        self.save_config_settings()
        if fname is None:
            self._collect_in_region = None
            return self.update_collect_colors()
        try:
            sf = shapefile.Reader(fname)
        except:
            msg = 'Could not open shapefile'
            dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
            dlg.ShowModal()
            dlg.Destroy()
            return self.update_collect_colors()


        shapes = sf.shapes()
        if len(shapes) == 1:
            shape = shapes[0]
            collect_in_region = shapely.geometry.asShape(shape)
        else:
            polygons = []
            for i in range(len(shapes)):
                polygons.append(shapely.geometry.asShape(shapes[i]))

            collect_in_region = shapely.geometry.MultiPolygon(polygons)

        centroid = np.array(collect_in_region.boundary.centroid.coords)
        print("centroid: ", centroid)
        max_lonlat = np.array([[180, 90]])
        if not np.all(np.abs(centroid) < max_lonlat):
            msg = 'Could not interpret shapefile, centroid is out of bounds: {}'.format(centroid[0].tolist())
            dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
            dlg.ShowModal()
            dlg.Destroy()
            return self.update_collect_colors()

        self._collect_in_region = collect_in_region

        return self.update_collect_colors()

    # ------------------------- END Camera Configuration -------------------------

    def on_toggle_saturated_pixels(self, event):
        """Toggle visilibity of uv row of images.

        """
        global show_satured_pixels
        show_satured_pixels = not show_satured_pixels

    def on_hot_key_help(self, event=None):
        """Show list of hot keys.

        """
        if not self._hot_key_list:
            self._hot_key_list = HotKeyList(self)

        self._hot_key_list.Raise()

    def on_menu_item_about(self, event):
        about_panel = wx.Panel(self, wx.ID_ANY)
        info = wx.AboutDialogInfo()
        info.Name = "KAMERA System Control Panel"
        info.Version = "0.0.0"
        info.Copyright = "(C) 2018 Kitware"
        info.Description = wordwrap(
            "This GUI allows control of the KAMERA system.",
            350, wx.ClientDC(self.ins_control_panel))
        info.WebSite = ("http://www.kitware.com", "Kitware")
        info.Developers = ["Matt Brown"]
        info.License = wordwrap(license_str, 500,
                                wx.ClientDC(about_panel))
        # Show the wx.AboutBox
        wx.AboutBox(info)
    # ------------------------------------------------------------------------


    # ------------------------------------------------------------------------

    def on_close_button(self, event=None):
        self.Close()
        if event is not None:
            event.Skip()

    def when_closed(self, event=None):
        #self.when_closed.Unbind(wx.EVT_CLOSE)
        #self.on_resize.Unbind(wx.EVT_SIZE)
        self.spatial_ins_interface.unregister()
        self.rgb_genicam_controller.unregister()
        self.timer.Unbind(wx.EVT_TIMER)
        try:
            self._metadata_entry_frame.Close()
        except:
            pass

        try:
            self._image_inspection_frame.Close()
        except:
            pass

        try:
            self._base_layer_frame.Close()
        except:
            pass

        event.Skip()


# === === === === === === end MainFrame === === === === === === ===


class CollectionModeFrame(form_builder_output_collection_mode.MainFrame):
    """.

    """
    def __init__(self, parent, collection_mode, collection_value, shapefile_fname,
                 save_mode_callback, shapefile_callback):
        """
        :param collection_mode:
        :type collection_mode: str

        :param collection_value:
        :type collection_value: str

        :param shapefile_fname: Currently loaded shapefile, or None
        :type shapefile_fname: Optional[str]

        :param save_mode_callback: Callback accepting collection mode string
            and associated float value.
        :type save_mode_callback: Callable

        :param shapefile_callback: Callback accepting the string
            filename for a shapefile to specify the collect-in region.
        :type shapefile_callback: Callable

        """
        form_builder_output_collection_mode.MainFrame.__init__(self, parent)

        if collection_mode == 'fixed rate':
            self.mode_combo_box.SetSelection(0)
            self.on_set_mode()
            try:
                collection_value = float(collection_value)
                self.rate_txt_ctrl.SetValue(str(collection_value))
            except Exception as e:
                exc_type, exc_obj, exc_tb = sys.exc_info()
                fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
                msg = "{}:{}\n{}: {}".format(fname, exc_tb.tb_lineno, exc_type.__name__, e)
                print(msg)

        else:
            if collection_mode == 'fixed overlap':
                self.mode_combo_box.SetSelection(1)
                self.on_set_mode()
                try:
                    collection_value = float(collection_value)
                    self.overlap_txt_ctrl.SetValue(str(collection_value))
                except Exception as e:
                    exc_type, exc_obj, exc_tb = sys.exc_info()
                    fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
                    msg = "{}:{}\n{}: {}".format(fname, exc_tb.tb_lineno, exc_type.__name__, e)
                    print(msg)

            else:
                self.save_button.Hide()
                self.percent_panel.Hide()
                self.rate_panel.Hide()

        if shapefile_fname is not None:
            self.shapefile_file_picker.SetPath(shapefile_fname)
            self.shapefile_checkbox.SetValue(True)
        self.save_mode_callback = save_mode_callback
        self.shapefile_callback = shapefile_callback
        self.shapefile_fname = shapefile_fname
        self.Show()
        self.SetMinSize(self.GetSize())

    def on_set_mode(self, event=None):
        ind = self.mode_combo_box.GetCurrentSelection()

        if ind == -1:
            self.save_button.Hide()
            self.percent_panel.Hide()
            self.rate_panel.Hide()
        elif ind == 0:
            self.rate_panel.Show()
            self.save_button.Show()
            self.percent_panel.Hide()
        elif ind == 1:
            self.rate_panel.Hide()
            self.save_button.Show()
            self.percent_panel.Show()

        self.percent_panel.GetParent().Layout()

    def on_select_shapefile(self, event):
        try:
            self.shapefile_fname = self.shapefile_file_picker.GetPath()
            self.shapefile_checkbox.SetValue(True)
        except:
            self.shapefile_fname = None
            self.shapefile_checkbox.SetValue(False)

        return

    def on_save(self, event):
        """When the 'Save' button is selected.

        """
        ind = self.mode_combo_box.GetCurrentSelection()
        err_msg = 'Must enter a value number'
        try:
            if ind == 0:
                mode = 'fixed rate'
                value = float(self.rate_txt_ctrl.GetValue())
                if value < 0 or value > G_MAX_FRAME_RATE:
                    err_msg = 'Frame rate must be in range 0-{} frames/second'.format(G_MAX_FRAME_RATE)
                    raise ValueError
            elif ind == 1:
                mode = 'fixed overlap'
                value = float(self.overlap_txt_ctrl.GetValue())
                if value > 99:
                    err_msg = 'Image overlap must be <99 percent'
                    raise ValueError
        except ValueError:
            dlg = wx.MessageDialog(self, err_msg, 'Error',
                                   wx.OK | wx.ICON_ERROR)
            dlg.ShowModal()
            dlg.Destroy()
            return

        if self.shapefile_checkbox.IsChecked():
            if self.shapefile_fname is not None:
                self.shapefile_callback(self.shapefile_fname)
            else:
                msg = 'Must select valid shapefile'
                dlg = wx.MessageDialog(self, msg, 'Error', wx.OK | wx.ICON_ERROR)
                dlg.ShowModal()
                dlg.Destroy()
        else:
            self.shapefile_callback(None)

        self.save_mode_callback(mode, value)
        self.Close()

    def on_cancel(self, event=None):
        """When the 'Cancel' button is selected.

        """
        self.Close()


class ImageInspectionFrame(form_builder_output_imagery_inspection.MainFrame):
    """Provides an orthographic base layer view of the region.

    """
    def __init__(self, parent, topic_names, save_config_callback,
                 stream=None, compressed=False):
        # type: (Any, dict, Callable, Any, bool) -> None
        """
        :param parent: Parent.
        :type parent: wx object

        :param effort_metadata_dict: Dictionary with key being the effort
            nickname and the value being a dictionary with keys 'project_name',
            'aircraft', 'flight', and 'field_notes'.
        :type effort_metadata_dict: dict

        :param effort_combo_box: Combo box for the collection event selection.
        :type effort_combo_box: wx.ComboBox

        :param edit_effort_name: Name of existing event that we want to edit.
        :type edit_effort_name: str

        """
        # Initialize parent class
        form_builder_output_imagery_inspection.MainFrame.__init__(self, parent)

        self.topic_names = topic_names
        self.save_config_callback = save_config_callback  # type: Callable
        self.full_view_rp = None
        self.compressed = compressed

        # Sets the scale factor that divides the integer slider value.
        self.contrast_slider_scale = 400

        global contrast_strength
        self.contrast_strength_slider.SetValue(contrast_strength*self.contrast_slider_scale)

        for i in range(self.image_stream_combo_box.GetCount()):
            if stream == self.image_stream_combo_box.GetString(i):
                self.image_stream_combo_box.SetSelection(i)
                wx.CallAfter(self.on_select_stream, None)

        self.Bind(wx.EVT_CLOSE, self.when_closed)
        self.on_contrast_strength()

    def on_select_stream(self, event=None):
        if self.full_view_rp is not None:
            self.full_view_rp.release()

        ind = self.image_stream_combo_box.GetCurrentSelection()

        if ind == 0:
            image_topic = self.topic_names['rgb_image_topic']
        elif ind == 1:
            raise Exception()

        srv_topic = '%s/view_service' % image_topic
        metadata = '%s/metadata_service' % image_topic

        self.full_view_rp = FullAndZoomView(self.full_view_panel,
                                            self.zoomed_view_panel,
                                            self.histogram_panel,
                                            self.zoom_slider,
                                            self.status_text, srv_topic,
                                            metadata, self.compressed)

    def on_toggle_saturated_pixels(self, event):
        """Toggle visilibity of uv row of images.

        """
        global show_satured_pixels
        show_satured_pixels = not show_satured_pixels

    def on_contrast_strength(self, event=None):
        global contrast_strength
        try:
            contrast_strength = float(self.contrast_strength_slider.GetValue())/self.contrast_slider_scale
            self.save_config_callback()
        except ValueError:
            pass

    def on_close_button(self, event=None):
        """When the 'Cancel' button is selected.

        """
        self.Close()

    def when_closed(self, event=None):
        if self.full_view_rp is not None:
            self.full_view_rp.release()
        event.Skip()


class SystemControlFrame(form_builder_output_system_control.MainFrame):
    """Provides an orthographic base layer view of the region.

    """
    def __init__(self, parent, camera_controllers, camera_names=None):
        """
        :param parent: Parent.
        :type parent: wx object

        :param effort_metadata_dict: Dictionary with key being the effort
            nickname and the value being a dictionary with keys 'project_name',
            'aircraft', 'flight', and 'field_notes'.
        :type effort_metadata_dict: dict

        :param effort_combo_box: Combo box for the collection event selection.
        :type effort_combo_box: wx.ComboBox

        :param edit_effort_name: Name of existing event that we want to edit.
        :type edit_effort_name: str

        """
        # Initialize parent class
        form_builder_output_system_control.MainFrame.__init__(self, parent)

        assert len(camera_controllers) > 0
        self.camera_controllers = camera_controllers

        self.camera_select_combo.SetEditable(True)
        self.camera_select_combo.Clear()

        if camera_names is None:
            camera_names = ['Camera %i' % (i + 1)
                            for i in range(len(camera_controllers))]

        for i in range(len(camera_controllers)):
            self.camera_select_combo.Append(camera_names[i])

        if False:
            if i == 0:
                self.camera_select_combo.Enable(False)

        self.camera_select_combo.SetSelection(0)
        self.camera_select_combo.SetEditable(False)

        self.Bind(wx.EVT_CLOSE, self.when_closed)

        self.main_panel.Refresh()

    @property
    def camera_controller(self):
        c = self.camera_controllers[self.camera_select_combo.GetSelection()]
        c.parent = self
        return c

    def get_auto_exposure_time_upper_limit(self, event=None):
        val = self.auto_exposure_time_upper_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.get_auto_exposure_time_upper_limit(val)
        if resp[0]:
            val = float(resp)/1000
            self.auto_exposure_time_upper_limit_response_txt_ctrl.SetLabel(val)
            self.auto_exposure_time_upper_limit_txt_ctrl.SetValue(val)

    def set_auto_exposure_time_upper_limit(self, event=None):
        val = self.auto_exposure_time_upper_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.set_auto_exposure_time_upper_limit(val)
        if resp[0]:
            self.auto_exposure_time_upper_limit_response_txt_ctrl.SetLabel(resp[1])

    def get_auto_exposure_time_lower_limit(self, event=None):
        val = self.auto_exposure_time_lower_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.get_auto_exposure_time_lower_limit(val)
        if resp[0]:
            val = float(resp)/1000
            self.auto_exposure_time_lower_limit_response_txt_ctrl.SetLabel(val)
            self.auto_exposure_time_lower_limit_txt_ctrl.SetValue(val)

    def set_auto_exposure_time_lower_limit(self, event=None):
        val = self.auto_exposure_time_lower_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.set_auto_exposure_time_lower_limit(val)
        if resp[0]:
            self.auto_exposure_time_lower_limit_response_txt_ctrl.SetLabel(
                resp[1])

    def get_auto_gain_upper_limit(self, event=None):
        val = self.auto_gain_upper_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.get_auto_gain_upper_limit(val)
        if resp[0]:
            val = float(resp)/1000
            self.auto_gain_upper_limit_response_txt_ctrl.SetLabel(val)
            self.auto_gain_upper_limit_txt_ctrl.SetValue(val)

    def set_auto_gain_upper_limit(self, event=None):
        val = self.auto_gain_upper_limit_txt_ctrl.GetValue()
        resp = self.camera_controller.set_auto_gain_upper_limit(val)
        if resp[0]:
            self.auto_gain_upper_limit_response_txt_ctrl.SetLabel(
                resp[1])

    def ptp_enable(self, event=None):
        self.camera_controller.set_ptp_enable()

    def when_closed(self, event=None):

        event.Skip()


class HotKeyList(form_builder_output_hot_key_list.MainFrame):
    def __init__(self, parent):
        """

        """
        # Initialize parent class
        form_builder_output_hot_key_list.MainFrame.__init__(self, parent)

        # ----------------------------- Hot Keys -----------------------------
        entries = [wx.AcceleratorEntry() for _ in range(1)]

        random_id = wx.NewId()
        self.Bind(wx.EVT_MENU, self.on_cancel, id=random_id)
        entries[0].Set(wx.ACCEL_NORMAL, wx.WXK_ESCAPE, random_id)

        accel = wx.AcceleratorTable(entries)
        self.SetAcceleratorTable(accel)
        # --------------------------------------------------------------------

        self.Show()
        self.SetMinSize(self.GetSize())

    def on_cancel(self, event=None):
        """When the 'Cancel' button is selected.

        """
        self.Close()


class BaseLayerPanelImage(object):
    """Base layer panel image.

    """
    def __init__(self, wx_panel, zoom_slider=None, zoom_spin_button=None,
                 zoom_label=None, show_ins_coord_sys=False):
        """
        :param wx_panel: Panel to add the image to.
        :type wx_panel: wx.Panel

        :param zoom: Percent zoom.
        :type zoom: float

        :param center: Full-image coordinates corresponding to the center of
            the zoom panel view.
        :type center: 2-array of float

        :param click_callback: Function to call when left-mouse is clicked. It
            should expect one arguement pos (the point clicked).

        """
        self.wx_panel = wx_panel
        self.raw_image = None
        self.image = None
        self.wx_image = None
        self.homography = None
        self.inverse_homography = None

        self.wx_panel.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)

        # Last known state.
        self.lat = None
        self.lon = None
        self.pos_cov = None
        self.roll = None
        self.pitch = None
        self.heading = None
        self.follow_system = True
        self.show_ins_coord_sys = show_ins_coord_sys

        self.lock = threading.RLock()
        self._system_center = None
        self._system_heading = None
        self.need_update = True
        self._dllh_de = None
        self._dllh_dn = None
        self.zoom_slider = zoom_slider
        self.zoom_spin_button = zoom_spin_button
        self.zoom_label = zoom_label

        self.wx_panel.Bind(wx.EVT_LEFT_DOWN, self.on_click)
        self.wx_panel.Bind(wx.EVT_RIGHT_DOWN, self.on_click)
        self.wx_panel.Bind(wx.EVT_PAINT, self.on_paint)
        self.wx_panel.Bind(wx.EVT_SIZE, self.on_size)

        self.zoom_slider.Bind(wx.EVT_SCROLL, self.on_zoom_slider)
        self.zoom_spin_button.Bind(wx.EVT_SPIN_DOWN, self.on_zoom_down)
        self.zoom_spin_button.Bind(wx.EVT_SPIN_UP, self.on_zoom_up)
        self.wx_panel.Bind(wx.EVT_MOUSEWHEEL, self.on_zoom_mouse_wheel)
        self.wx_panel.Bind(wx.EVT_IDLE, self.on_idle)

    @property
    def dllh_de(self):
        """Change in latitude (deg), longitude (deg), and height above
        ellipsoid from moving one meter east at the center of the base layer.

        """
        return self._dllh_de

    @property
    def dllh_dn(self):
        """Change in latitude (deg), longitude (deg), and height above
        ellipsoid from moving one meter north at the center of the base layer.

        """
        return self._dllh_dn

    @property
    def zoom(self):
        im_height, im_width = self.raw_image.shape[:2]
        panel_width, panel_height = self.wx_panel.GetSize()
        fract = np.minimum(panel_width/im_width, panel_height/im_height)
        return self.zoom_fract*fract*100

    @property
    def zoom_fract(self):
        """Zoom as a fraction of panel size.

        Zoom fraction is defined:

          (raw-image columns) / (columns of raw-image shown)

          or

          (raw-image rows) / (rows of raw-image shown)

        whichever is smaller.

        """
        return self._zoom_fract

    @property
    def zoom_fract_max(self):
        # The minimum zoom_fraction is 1, which shows the entire image in the
        # panel. The maximum zoom is such that zoom is 1000%.
        im_height, im_width = self.raw_image.shape[:2]
        panel_width, panel_height = self.wx_panel.GetSize()
        fract = np.minimum(panel_width/im_width, panel_height/im_height)
        return 10/fract

    def set_ins_state(self, lat=None, lon=None, pos_cov=None, roll=None,
                      pitch=None, heading=None):
        #print(lat, lon, pos_cov, roll, pitch, heading)
        with self.lock:
            if lat is not None:
                self.lat = lat

            if lon is not None:
                self.lon = lon

            if pos_cov is not None:
                self.pos_cov = pos_cov

            if roll is not None:
                self.roll = roll

            if pitch is not None:
                self.pitch = pitch

            if heading is not None:
                self.heading = heading

            if (self.follow_system and lat is not None and lon is not None
                and self.homography is not None):
                pos = self.get_im_pt_from_lon_lat(lon, lat)
                self.set_center(pos)

    def on_idle(self, event=None):
        if self.need_update:
            self.update_all_if_needed()
            self.need_update = False

        time.sleep(0.01)

    def set_zoom_fract(self, zoom_fract, update_slider=True):
        """
        :param zoom_fract: Zoom fraction.
        :type zoom_fract: float

        """
        self._zoom_fract = zoom_fract

        if update_slider:
            # See on_zoom_slider for documentation of these operations.
            # Want to smooth out the sensitivity.
            zoom_slider_float = (zoom_fract - 1)/(self.zoom_fract_max - 1)
            zoom_slider_float = np.maximum(0, zoom_slider_float)
            zoom_slider_float = np.sqrt(zoom_slider_float)
            zoom_slider_float = np.minimum(zoom_slider_float, 1)
            self.zoom_slider.SetValue(10000*zoom_slider_float)

        self.need_update = True

    def set_center(self, center):
        """
        :param center: Location for the zoom center in the original image's
            coordinates.
        :type center: 2-array

        """
        # If the center is off of the image, ignore the requested update.
        im_height, im_width = self.raw_image.shape[:2]
        if center[0] < 0 or center[1] < 0 or \
           center[0] > im_width or center[1] > im_height:
            return

        self._center = center

        self.need_update = True

    def on_zoom_mouse_wheel(self, event=None):
        val = event.GetWheelRotation()
        if event.ShiftDown():
            change = 1.1
        else:
            change = 1.01

        if val > 0:
            self.on_zoom_up(change=change)
        if val < 0:
            self.on_zoom_down(change=change)

    def on_zoom_slider(self, event=None):
        # The zoom slider is set to return between 0 and 10,000.
        zoom_slider_float = self.zoom_slider.GetValue()/10000.0

        # Want to smooth out the sensitivity.
        zoom_slider_float = zoom_slider_float**2

        zoom_fract = 1 + (self.zoom_fract_max - 1)*zoom_slider_float
        self.set_zoom_fract(zoom_fract, update_slider=False)

    def on_zoom_up(self, event=None, change=1.02):
        self.set_zoom_fract(self.zoom_fract*change)

    def on_zoom_down(self, event=None, change=1.02):
        self.set_zoom_fract(self.zoom_fract/change)

    def update_raw_image(self, raw_image):
        """Replace raw_image and update the rendered view in the panel.

        """
        self.raw_image = raw_image
        self.update_all()

    def on_size(self, event):
        """Called on event wx.EVT_SIZE.

        """
        self.update_all()

    def update_all(self):
        self.need_update = True

    def update_all_if_needed(self):
        with self.lock:
            if self.raw_image is not None:
                #print('on_size')
                panel_width, panel_height = self.wx_panel.GetSize()
                self.wx_image = wx.EmptyImage(panel_width, panel_height)

                # A bit out of place here, but this method is called whenever
                # an action occurs that changes how the image is drawn. The
                # zoom percent displayed is in terms of the percent of native
                # resolution, so when the frame is resized, this displayed
                # percent needs to be updated. Also, we want to be sure that
                # always stay within the minimum and
                # maximum for zoom_fract.
                zoom_fract = np.maximum(1, self.zoom_fract)
                zoom_fract = np.minimum(zoom_fract, zoom_fract)
                self.set_zoom_fract(zoom_fract, update_slider=True)
                self.zoom_label.SetLabel('{}%'.format(int(np.round(self.zoom))))

                # So that the layout adjusts for changes in the text size.
                self.zoom_label.GetParent().Layout()

                panel_width, panel_height = self.wx_panel.GetSize()

                # ----------------------------------------------------------------
                if self.raw_image is not None:
                    s = self.zoom/100
                    tx = panel_width/2-s*self._center[0]
                    ty = panel_height/2-s*self._center[1]
                    self.homography = np.array([[s,0,tx],[0,s,ty],[0,0,1]])
                    self.inverse_homography = np.linalg.inv(self.homography)

                    panel_width, panel_height = self.wx_panel.GetSize()
                    flags = cv2.INTER_CUBIC | cv2.WARP_INVERSE_MAP
                    image = cv2.warpPerspective(self.raw_image,
                                                self.inverse_homography,
                                                dsize=(panel_width, panel_height),
                                                flags=flags)

                    if image.ndim == 2:
                        image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB)

                    self.wx_image.SetData(image.tostring())
                    self.wx_bitmap = self.wx_image.ConvertToBitmap()
                else:
                    self.wx_image = None
                # ----------------------------------------------------------------

                self.wx_panel.Refresh(True)
            else:
                self.homography = None
                self.inverse_homography = None
                self.wx_image = None
                self.wx_panel.Refresh(True)

    def on_paint(self, event=None):
        """Called on event wx.EVT_PAINT.

        """
        with self.lock:
            if self.wx_image is not None:
                # AutoBufferedPaintDC helps avoid flicker.
                if False:
                    dc = wx.PaintDC(self.wx_panel)
                else:
                    pdc = wx.PaintDC(self.wx_panel)
                    dc = wx.GCDC(pdc)

                #dc = wx.BufferedDC(dc)
                dc.DrawBitmap(self.wx_bitmap, 0,0)
                self.draw_overlay(dc)

            if event is not None:
                event.Skip()

    def refresh(self, event=None):
        """Useful to bind the Refresh of self.wx_panel to an event.

        """
        self.wx_panel.Refresh(True)
        if event is not None:
            event.Skip()

    def draw_overlay(self, dc):
        with self.lock:
            # Convert from lon/lat to panel image coordinates.
            lon = self.lon
            lat = self.lat
            pos_cov = self.pos_cov

            if (lon is not None and lat is not None and
                self.homography is not None):
                pt0 = self.get_im_pt_from_lon_lat(self.lon, self.lat)
                pt = np.dot(self.homography, [pt0[0], pt0[1], 1])
                center = pt[:2]/pt[2]

                dc.SetPen(wx.Pen([50, 50, 255], 2))
                dc.SetBrush(wx.Brush(wx.Colour(50, 50, 255)))
                dc.DrawCircle(center[0], center[1], 8)

                # Calculate the derivatives for change in pixel (x,y)
                # as a function de,dn in meters.
                im_pt_de = self.get_im_pt_from_lon_lat(self.dllh_de[1] + lon,
                                                       self.dllh_de[0] + lat)
                im_pt_dn = self.get_im_pt_from_lon_lat(self.dllh_dn[1] + lon,
                                                       self.dllh_dn[0] + lat)

                im_pt_de = im_pt_de - pt0
                im_pt_dn = im_pt_dn - pt0

                # Account for the shrinking of the base layer to fit in
                # the panel.
                J = np.vstack([im_pt_de, im_pt_dn]).T
                J = np.dot(self.homography[:2, :2], J)

                # J is now the Jacobian matrix for (x,y) = f(e, n).
                #    | dx/de dx/dn |
                #    | dy/de dy/dn |

                # Columns are vectors along the primary axes of the
                # ellipsoid axes in east/north meters.
                variance, el_axes = np.linalg.eig(pos_cov)

                for i in range(len(variance)):
                    el_axes[:,i] *= np.sqrt(variance[i])

                # Spoof to meter.
                #el_axes = np.identity(2)*10

                # Convert axes to x,y pixel displacement
                el_axes = np.dot(J, el_axes)

                #print(np.sqrt(np.sum(el_axes**2, 0)))

                # Show 2 standard deviations surface.
                el_axes *= 2.0

                # Points along the ellpse.
                N = 50
                theta = np.linspace(0, 2*np.pi, N)
                pts = np.zeros((2, N))
                pts[0] = np.cos(theta)*el_axes[0,0] + np.sin(theta)*el_axes[0,1] + center[0]
                pts[1] = np.cos(theta)*el_axes[1,0] + np.sin(theta)*el_axes[1,1] + center[1]

                lines = [(pts[0, j], pts[1, j], pts[0, j+1], pts[1, j+1])
                         for j in range(pts.shape[1]-1)]

                # Draw a line to the last bounding box.
                dc.SetPen(wx.Pen([255, 0, 0], width=5))

                dc.DrawLineList(lines)

                if (self.show_ins_coord_sys and self.roll is not None and
                    self.pitch is not None and self.heading is not None):
                    # R is with respect to a NED coordinate system here.
                    R_ned = euler_matrix(self.heading/180*np.pi,
                                         self.pitch/180*np.pi,
                                         self.roll/180*np.pi,
                                         axes='rzyx')[:3, :3].T

                    # Caluclate the directions in ENU.
                    xdir = R_ned[0, 1], R_ned[0, 0]
                    ydir = R_ned[1, 1], R_ned[1, 0]
                    zdir = R_ned[2, 1], R_ned[2, 0]

                    # Convert from ENU meters to panel coordinates.
                    J = J/np.linalg.norm(J)
                    xdir = np.dot(J, xdir)
                    ydir = np.dot(J, ydir)
                    zdir = np.dot(J, zdir)


                    r = 200 # panel pixels
                    xdir *= r
                    ydir *= r
                    zdir *= r

                    # X-coordinate system direction.
                    lines = [(center[0], center[1],
                              center[0] + xdir[0], center[1] + xdir[1])]
                    dc.SetPen(wx.Pen([255, 0, 0], width=5))
                    dc.DrawLineList(lines)

                    # X-coordinate system direction.
                    lines = [(center[0], center[1],
                              center[0] + ydir[0], center[1] + ydir[1])]
                    dc.SetPen(wx.Pen([0, 255, 0], width=5))
                    dc.DrawLineList(lines)

                    # X-coordinate system direction.
                    lines = [(center[0], center[1],
                              center[0] + zdir[0], center[1] + zdir[1])]
                    dc.SetPen(wx.Pen([0, 0, 255], width=5))
                    dc.DrawLineList(lines)


    def load_geotiff(self, fname):
        if fname is None:
            return

        if not os.path.isfile(fname):
            print('Could not open file \'%s\'' % fname)
            return

        ds = gdal.Open(fname, gdal.GA_ReadOnly)
        if ds.RasterCount > 1:
            raw_image = np.zeros((ds.RasterYSize, ds.RasterXSize,3),
                                 dtype=np.uint8)
            for i in range(3):
                raw_image[:, :, i] = ds.GetRasterBand(i+1).ReadAsArray()
        else:
            band = ds.GetRasterBand(1)
            raw_image = band.ReadAsArray()

        # Create lat/lon coordinate system.
        wgs84_wkt = """
        GEOGCS["WGS 84",
            DATUM["WGS_1984",
                SPHEROID["WGS 84",6378137,298.257223563,
                    AUTHORITY["EPSG","7030"]],
                AUTHORITY["EPSG","6326"]],
            PRIMEM["Greenwich",0,
                AUTHORITY["EPSG","8901"]],
            UNIT["degree",0.01745329251994328,
                AUTHORITY["EPSG","9122"]],
            AUTHORITY["EPSG","4326"]]"""
        wgs84_cs = osr.SpatialReference()
        wgs84_cs.ImportFromWkt(wgs84_wkt)

        image_cs= osr.SpatialReference()
        image_cs.ImportFromWkt(ds.GetProjectionRef())

        # Create a transform object to convert between projection of the image
        # and WGS84 coordinates.
        self._to_lla_tform = osr.CoordinateTransformation(image_cs, wgs84_cs)
        self._from_lla_tform = osr.CoordinateTransformation(wgs84_cs, image_cs)

        # See documentation for gdal GetGeoTransform().
        geo_tform_mat = np.identity(3)
        c = ds.GetGeoTransform()
        geo_tform_mat[0,0] = c[1]
        geo_tform_mat[0,1] = c[2]
        geo_tform_mat[0,2] = c[0]
        geo_tform_mat[1,0] = c[4]
        geo_tform_mat[1,1] = c[5]
        geo_tform_mat[1,2] = c[3]
        self.geo_tform_mat = geo_tform_mat[:2]
        self.geo_inv_tform_mat = np.linalg.inv(geo_tform_mat)[:2]

        self._center = np.array(raw_image.shape[:2][::-1])/2
        self._zoom_fract = 10

        self._system_center = None

        lon0, lat0 = self.get_lon_lat_from_im_pt([raw_image.shape[1]/2,
                                                  raw_image.shape[0]/2])
        llh0 = np.array([lat0, lon0, 0])
        self._dllh_de = np.array(enu_to_llh(1, 0, 0, lat0, lon0, 0)) - llh0
        self._dllh_dn = np.array(enu_to_llh(0, 1, 0, lat0, lon0, 0)) - llh0

        # This will cause the panel to update.
        self.update_raw_image(raw_image)

    def get_lon_lat_from_im_pt(self, im_pt):
        """Return latitude and longitude for panel image coordinates.

        :param pos: Raw image coordinates of the geotiff that were clicked.
        :type pos: 2-array

        :return: Longitude (degrees) and latitude (degrees) associated with
            the clicked point.
        :rtype: 3-array

        """
        # Convert from image coordinates to the coordinates of the projection
        # encoded in the geotiff.
        Xp, Yp = np.dot(self.geo_tform_mat, [im_pt[0], im_pt[1], 1])
        lon, lat, _ = self._to_lla_tform.TransformPoint(Xp, Yp)
        return lon, lat

    def get_im_pt_from_lon_lat(self, lon, lat):
        """Return panel image coordinates for latitude and longitude.

        :param lon: Longitude (degrees).
        :type lon: float

        :param lat: Latitude (degrees).
        :type lat: float

        :return: Raw image coordinates of the geotiff.
        :rtype: 2-array

        """
        Xp, Yp,_ = self._from_lla_tform.TransformPoint(lon, lat)

        # Convert from image coordinates to the coordinates of the projection
        # encoded in the geotiff.
        im_pt = np.dot(self.geo_inv_tform_mat, [Xp, Yp, 1])
        return im_pt

    def on_click(self, event):
        """Called on events wx.EVT_RIGHT_DOWN or wx.EVT_LEFT_DOWN.

        """
        if self.raw_image is not None and self.inverse_homography is not None:
            pos = event.GetPosition()
            pos = np.dot(self.inverse_homography, [pos[0],pos[1],1])
            pos = pos[:2]/pos[2]

            # We don't want the image going off of the screen, so pos should be
            # clamped to the corners if necassary.
            im_height, im_width = self.raw_image.shape[:2]
            pos[0] = np.maximum(pos[0], 0)
            pos[1] = np.maximum(pos[1], 0)
            pos[0] = np.minimum(pos[0], im_width)
            pos[1] = np.minimum(pos[1], im_height)

            self.set_center(pos)


class BaseLayerFrame(form_builder_output_base_layer.MainFrame):
    """Provides an orthographic base layer view of the region.

    """
    def __init__(self, parent, window_title='Base Layer'):
        """
        :param detection_database: Read-only instance of the detection
            database.
        :type detection_database: DetectionDatabase

        :param select_track_callback: Function to call when a detection
            is selected with single string arguement of the detection's uid.
        :type select_track_callback: function

        :param track_dict: Track dictionary mapping track UID to the Track
            message.
        :type track_dict: dictionary

        :param only_show_track_head: Only show the bounding box for the head of
            the track track.
        :type only_show_track_head: bool

        :param base_layer_filename: Path to base layer geoTIFF.
        :type base_layer_filename: str

        """
        # Initialize parent class
        form_builder_output_base_layer.MainFrame.__init__(self, parent)
        self.SetTitle(window_title)

        # Create the BaseLayerPanelImage that is responsible for displaying the
        # base layer, drawing overlays, and interacting with clicked poitns.
        self.base_layer_image = BaseLayerPanelImage(self.base_layer_panel,
                                                    zoom_slider=self.zoom_slider,
                                                    zoom_spin_button=self.zoom_spin_button,
                                                    zoom_label=self.zoom_static_text,
                                                    show_ins_coord_sys=self.show_ins_coord_sys_checkbox.GetValue())


        self.base_layer_image.load_geotiff('/home/local/KHQ/matt.brown/adapt/adapt_ros_ws/data/rotherham.tif')

        self.Show()
        self.SetMinSize(self.GetSize())

    def update_all(self):
        self.base_layer_image.need_update = True

    def on_load_image(self, event):
        fdlg = wx.FileDialog(self, 'Select a geoTIFF.')
        if fdlg.ShowModal() == wx.ID_OK:
            file_path = fdlg.GetPath()
        else:
            return

        self.base_layer_image.load_geotiff(file_path)

    def on_follow_system_checkbox(self, event):
        self.base_layer_image.follow_system = self.follow_system_checkbox.GetValue()

    def on_show_ins_coord_sys_checkbox(self, event):
        self.base_layer_image.show_ins_coord_sys = self.show_ins_coord_sys_checkbox.GetValue()
        self.base_layer_image.refresh()

    def set_ins_state(self, lat=None, lon=None, pos_cov=None, roll=None,
                      pitch=None, heading=None):
        self.base_layer_image.set_ins_state(lat, lon, pos_cov, roll, pitch,
                                            heading)
