#***************************************************************************
#                      Copyright Jaime Machuca
#***************************************************************************
# Title        : sc_SonyQX1.py
#
# Description  : This file contains a class to use the Sony QX range of cams
#                It finds a camera using SSDP discovery and returns it as an
#                object. If a camera is not found it returns an error value
#                that should be catched by the application. Initially it will
#                have support for triggering the camera, and downloading the
#                latest image file. Other functions will be added gradually.
#
# Environment  : Python 2.7 Code. Intended to be included in a Mavproxy Module
#
# Responsible  : Jaime Machuca
#
# License      : CC BY-NC-SA
#
# Editor Used  : Xcode 6.1.1 (6A2008a)
#
#****************************************************************************

#****************************************************************************
# HEADER-FILES (Only those that are needed in this file)
#****************************************************************************

# System Header files and Module Headers
import os, sys, time, math, cv2, struct, fcntl
from datetime import datetime

# Module Dependent Headers
import requests, json, socket, StringIO
import xml.etree.ElementTree as ET

# Own Headers
import ssdp

#****************************************************************************
# Constants
#****************************************************************************

# Target Initial Camera Values
targetShutterSpeed = 1600
targetAperture = 120
targetISOValue = "AUTO"

#****************************************************************************
# Class name       : SmartCamera_SonyQX
#
# Public Methods   : boGetLatestImage
#                    u32GetImageCounter
#                    boTakePicture
#                    boSetExposureMode
#                    boSetShutterSpeed
#                    boSetAperture
#                    boSetISO
#                    boZoomIn
#                    boZoomOut
#
# Private Methods  : __sFindInterfaceIPAddress
#                    __sFindCameraURL
#                    __sMakeCall
#                    __sSimpleCall
#****************************************************************************
class SmartCamera_SonyQX():

#****************************************************************************
#   Method Name     : __init__ Class Initializer
#
#   Description     : Initializes the class
#
#   Parameters      : u8instance        Camera Instance Number
#                     snetInterface     String containing the Network Interface
#                                       Name where we should look for the cam
#
#   Return Value    : None
#
#   Author           : Jaime Machuca, Randy Mackay
#
#****************************************************************************

    def __init__(self, u8Instance, sNetInterface):

        # record instance
        self.u8Instance = u8Instance
        self.sConfigGroup = "Camera%d" % self.u8Instance

        # background image processing variables
        self.u32ImgCounter = 0              # num images requested so far

        # latest image captured
        self.sLatestImageURL = None         # String with the URL to the latest image

        # latest image downloaded
        self.sLatestImageFilename = None    #String with the Filename for the last downloaded image
        self.sLatestFileName = None         #String with the camera file name for the last image taken

        self.vehicleLat = 0.0              # Current Vehicle Latitude
        self.vehicleLon = 0.0              # Current Vehicle Longitude
        self.vehicleHdg = 0.0              # Current Vehicle Heading
        self.vehicleAMSL = 0.0             # Current Vehicle Altitude above mean sea level

        self.vehicleRoll = 0.0              # Current Vehicle Roll
        self.vehiclePitch = 0.0              # Current Vehicle Pitch


        # Look Camera and Get URL
        self.sCameraURL = self.__sFindCameraURL(sNetInterface)
        if self.sCameraURL is None:
            print("No QX camera found, failed to open QX camera %d" % self.u8Instance)
        else:
            self.__openGeoTagLogFile()      # open geoTag Log
            self.boCameraInitialSetup()     # Setup Initial camera parameters

#****************************************************************************
#   Method Name     : __str__
#
#   Description     : Returns a human readable string name for the instance
#
#   Parameters      : none
#
#   Return Value    : String with object instance name
#
#   Author           : Randy Mackay
#
#****************************************************************************

    # __str__ - print position vector as string
    def __str__(self):
        return "SmartCameraSonyQX Object for %s" % self.sConfigGroup

#****************************************************************************
#   Method Name     : boCameraInitialSetup
#
#   Description     : Sets Initial Camera Parameters
#
#   Parameters      : None
#
#   Return Value    : None
#
#   Author           : Jaime Machuca
#
#****************************************************************************

    def boCameraInitialSetup(self):
        print("Setting up Camera Initial Parameters")
        # Check if we need to do 'startRecMode'
        APIList = self.__sSimpleCall("getAvailableApiList")

        # For those cameras which need it
        if 'startRecMode' in (APIList['result'])[0]:
            print("Need to send startRecMode, sending and waiting 5 sec...")
            self.__sSimpleCall("startRecMode")
            time.sleep(1)
            print("4 sec")
            time.sleep(1)
            print("3 sec")
            time.sleep(1)
            print("2 sec")
      
      time.sleep(1)
            print("1 sec")
            time.sleep(1)

        # Set Postview Size to Orignial size to get real image filename
        sResponse = self.__sSimpleCall("getSupportedPostviewImageSize")
        print("%s" % sResponse)

        sResponse = self.__sSimpleCall("setPostviewImageSize", adictParams=["Original"])
        print("%s" % sResponse)

        s
Response = self.__sSimpleCall("getPostviewImageSize")
        print("%s" % sResponse)

        # Set Mode to Shutter Priority if available
        SupportedModes = self.__sSimpleCall("getSupportedExposureMode")
        if 'Shutter' in (SupportedModes['result'])[0]:
            self.boSetExposureMode("Shutter")
        #elif 'Manual' in (SupportedModes['result'])[0]:
        #    self.boSetExposureMode("Manual")
        else:
            print("Error no Shutter Priority Mode")

        # Set Target Shutter Speed
        self.boSetShutterSpeed(targetShutterSpeed)

        # Set Target ISO Value
        self.boSetISO(targetISOValue)

        #Take Confirmation Picture
        self.boTakePicture()

#****************************************************************************
#   Method Name     : boSet_GPS
#
#   Description     : Gets the GPS Position from the provided message
#
#   Parameters      : mGPSMessage       GPS Mavlink Message type
#
#
#   Return Value    : None
#
#   Author           : Jaime Machuca
#
#****************************************************************************

    def boSet_GPS(self, mGPSMessage):
        if mGPSMessage.get_type() == 'GLOBAL_POSITION_INT':
            (self.vehicleLat, self.vehicleLon, self.vehicleHdg, self.vehicleAMSL) = (mGPSMessage.lat*1.0e-7, mGPSMessage.lon*1.0e-7, mGPSMessage.hdg*0.01, mGPSMessage.alt*0.001)

#****************************************************************************
#   Method Name     : boSet_Attitude
#
#   Description     : Gets the vehicle attitude from the provided message
#
#   Parameters      : mAttitudeMessage  MAVlink Attitude Message type
#
#
#   Return Value    : None
#
#   Author           : Jaime Machuca
#
#****************************************************************************

    def boSet_Attitude(self, mAttitudeMessage):
        if mAttitudeMessage.get_type() == 'ATTITUDE':
        