"""
wc rc2.py: 

Backend for WHOZ CHILLIN hackathon project

"""

# Random shit we need to import

import os, sys
from dronekit import connect, VehicleMode, LocationGlobalRelative
# This sys.path.insert is needed to direct WHOZ CHILLIN to the mavlink-solo-master rather than pymavlink.
# This is commented out to run on Solo, I think.
# sys.path.insert(0, '/Users/danielmckinnon/Documents/hackathon/mavlink-solo-master')
from pymavlink import mavutil # Needed for command message definitions
import time
import math
import argparse
import urllib

# Constants

EARTH_RADIUS = 6371010.0
MAVLINK_GIMBAL_SYSTEM_ID = 1
MAVLINK_GIMBAL_COMPONENT_ID = 154

# Matrix of WHOZ CHILLIN locations

CHILLIN_locales = {

"DANSROOF": LocationGlobalRelative(37.866115, -122.301144, 100),
"TACOTRUCK": LocationGlobalRelative(37.872848, -122.301515, 100),
"BALLIN": LocationGlobalRelative(37.872355, -122.29756, 100),
"SKATIN": LocationGlobalRelative(37.881031, -122.303447, 100),
"THREEDR": LocationGlobalRelative(37.873424, -122.302561, 100),

}

# Matrix Arducopter failsafe behaviors

FS_tables = {

0: "Disabled",
1: "Enabled, always RTL",
2: "Enabled, continue with Mission in Auto",
3: "Enabled, always land",

}

#Math to determine distance between two locations on earth

def getDistanceFromPoints(a, b):
    # Get the difference between our two points then convert the difference into radians
    nDLat = math.radians(a.lat - b.lat)
    nDLon = math.radians(a.lon - b.lon)
    
    fromLat =  math.radians(b.lat)
    toLat =  math.radians(a.lat)
    
    lat2 = math.sin(nDLat/2) * math.sin(nDLat/2)
    long2 = math.sin(nDLon/2) * math.sin(nDLon/2)
    nA = lat2 + math.cos(fromLat) * math.cos(toLat) * long2
    
    nC = 2 * math.atan2( math.sqrt(nA), math.sqrt( 1 - nA ))
    nD = EARTH_RADIUS * nC

    return nD

# Function to take a full resolution GoPro picture that will be stored on the SD card. GoPro must be in Photo mode or a video will be taken.

def take_a_pic():

    print "take_a_pic()"

    msg = vehicle.message_factory.gopro_set_request_encode(MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))

    print "msg: ", msg

    vehicle.send_mavlink(msg)
    vehicle.flush()

    print "sent message: ", msg

def take_live_pic():

    # Run "solo video acquire" 

    # Run "solo video restore" to restore live stream

    # Run "modprobe mxc_v4l2_capture" at every startup

    gst-launch tvsrc device=/dev/video0 ! jpegenc ! tcpserversink port=5000 sync=false

def horizon_point():

    vehicle.flush()
    
    msg = vehicle.message_factory.mount_configure_encode(
            0, 1,    # target system, target component
            mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,  #mount_mode
            1,  # stabilize roll
            1,  # stabilize pitch
            1,  # stabilize yaw
            )
    
    vehicle.send_mavlink(msg)
    vehicle.flush()

    msg = vehicle.message_factory.mount_control_encode(
            0, 1,    # target system, target component
            0*100, # pitch is in centidegrees
            0.0, # roll
            0, # yaw is in centidegrees
            0 # save position
            )

    print "mavlink message is:", msg

    vehicle.send_mavlink(msg)
    vehicle.flush()

def nadir_point():

    vehicle.flush()
    
    msg = vehicle.message_factory.mount_configure_encode(
            0, 1,    # target system, target component
            mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,  #mount_mode
            1,  # stabilize roll
            1,  # stabilize pitch
            1,  # stabilize yaw
            )
    
    vehicle.send_mavlink(msg)
    vehicle.flush()

    msg = vehicle.message_factory.mount_control_encode(
            0, 1,    # target system, target component
            -90*100, # pitch is in centidegrees
            0.0, # roll
            0, # yaw is in centidegrees
            0 # save position
            )

    print "mavlink message is:", msg

    vehicle.send_mavlink(msg)
    vehicle.flush()

# Connect to vehicle.

parser = argparse.ArgumentParser(description='Print out vehicle state information. Connects to SITL on local PC by default.')
parser.add_argument('--connect', default='127.0.0.1:14550', help="vehicle connection target. Default '127.0.0.1:14550'")
parser.add_argument('--chillin', default='DANSROOF', help="WHOZ CHILLIN target. Default: DANSROOF. Options: TACOTRUCK, SKATIN, BALLIN")
args = parser.parse_args()
user_CHILLIN = CHILLIN_locales[args.chillin]

print "user_CHILLIN is: ", user_CHILLIN

# Connect to the Vehicle. 
# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.
print "\nConnecting to vehicle on: %s" % args.connect
vehicle = connect(args.connect, wait_ready=True)

# Get Vehicle Home location - will be `None` until first set by autopilot
while not vehicle.home_location:
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()
    if not vehicle.home_location:
        print " Waiting for home location ..."

# We have a home location, so print it!        
print "\n Home location: %s" % vehicle.home_location

# Switch GoPro to photo mode

print("Switching to photo")

msg = vehicle.message_factory.gopro_set_request_encode(MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (1, 0, 0, 0))

print("msg:", msg)

vehicle.send_mavlink(msg)
vehicle.flush()

# Point gimbal to horizon

horizon_point()

print "Gimbal pointed toward horizon"

# Set vehicle RTL behavior so it can complete a mission with returning home

print "FS_THR_ENABLE was: ", FS_tables[vehicle.parameters['FS_THR_ENABLE']]

vehicle.parameters['FS_THR_ENABLE'] = 0

print "FS_THR_ENABLE is", FS_tables[vehicle.parameters['FS_THR_ENABLE']]

# Make WHOZ CHILLIN Solo get there fast

print "WPNAV_SPEED: %d cm/s" % vehicle.parameters['WPNAV_SPEED']

vehicle.parameters['WPNAV_SPEED'] = 2000

print "WPNAV_SPEED %d cm/s" % vehicle.parameters['WPNAV_SPEED']

# Set mode to LOITER

print "Set mode=LOITER (currently: %s) and wait for callback" % vehicle.mode.name 
vehicle.mode = VehicleMode("LOITER")

while vehicle.mode != VehicleMode('LOITER'):
    print 'waiting for callback...'
    time.sleep(1)

print "Arming vehicle"
vehicle.armed = True

while not vehicle.armed:
    print 'waiting for arming...'
    time.sleep(1)

print "vehicle is armed!"
print ''

# Set mode to GUIDED

print "Set mode=GUIDED (currently: %s) and wait for callback" % vehicle.mode.name 
vehicle.mode = VehicleMode("GUIDED")

while vehicle.mode != VehicleMode('GUIDED'):
    print 'waiting for callback...'
    time.sleep(1)

# # Retry a few times because why not
# print 'taking off'
# i = 5
# while i > 0:
#     vehicle.simple_takeoff(100.0)
#     time.sleep(1)
#     i -= 1

vehicle.simple_takeoff(100)

time.sleep(5)

# Point gimbal toward nadir

nadir_point()

print "Gimbal pointed toward nadir."

while vehicle.location.global_relative_frame.alt<0.95*100:
        print "Climbing to missions altitude: ", vehicle.location.global_relative_frame.alt
        print user_CHILLIN   
        time.sleep(1)

print ''

print 'Attempting to navigate to location ', user_CHILLIN

# Navigate to CHILLIN location

vehicle.simple_goto(user_CHILLIN)

# Wait until vehicle gets there
    
while getDistanceFromPoints(vehicle.location.global_frame, user_CHILLIN) > 2:
    print "In transit. CQ will arrive shortly."
    print vehicle.location.global_frame
    print user_CHILLIN
    print getDistanceFromPoints(vehicle.location.global_frame, user_CHILLIN)
    time.sleep(2)

# Take four pictures from four different headings of CHILLIN location

print("Imaging CHILLIN location in process.")

angles = [0, 90, 180, 270]

for i in angles:

    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
        0, #confirmation
        i,    # param 1, yaw in degrees
        45,          # param 2, yaw speed deg/s
        1,          # param 3, direction -1 ccw, 1 cw
        0, # param 4, relative offset 1, absolute angle 0
        0, 0, 0)    # param 5 ~ 7 not used
    # send command to vehicle

    vehicle.send_mavlink(msg)
    vehicle.flush()

    time.sleep(5)

    take_a_pic()

    time.sleep(5)

vehicle.simple_goto(CHILLIN_locales["THREEDR"])

# Point gimbal to horizon

horizon_point()

print "Gimbal pointed toward horizon"

# Wait until vehicle gets there
while getDistanceFromPoints(vehicle.location.global_frame, CHILLIN_locales["THREEDR"]) > 2:
        print " In transit"
        print vehicle.location.global_frame
        print CHILLIN_locales["THREEDR"]
        print getDistanceFromPoints(vehicle.location.global_frame, CHILLIN_locales["THREEDR"])
        time.sleep(2)

print "Returning to launch"
vehicle.mode    = VehicleMode("RTL")



#PRECISION LAND


# Return vehicle failsafe behavior to default

vehicle.parameters['FS_THR_ENABLE'] = 1

print "FS_THR_ENABLE has been returned to:", FS_tables[vehicle.parameters['FS_THR_ENABLE']]

# Return speed to normal

print "WPNAV_SPEED: %d cm/s" % vehicle.parameters['WPNAV_SPEED']

vehicle.parameters['WPNAV_SPEED'] = 1000

print "WPNAV_SPEED %d cm/s" % vehicle.parameters['WPNAV_SPEED']

#Close vehicle object before exiting script
print "\nClose vehicle object"
vehicle.close()



print("Completed")


# To begin SITL, run "dronekit-sitl copter-3.3 --home=37.873894,-122.302141,584,353" from the terminal
# Boot up MavProxy using "mavproxy.py --master=tcp:127.0.0.1:5760 --out=udpout:10.1.49.130:14550 --out=udpout:127.0.0.1:14550 --out=udpout:127.0.0.1:1549"
# The first UDP out is for an my Android tablet so I can use Tower, the second is this Python script, and the third is for Tim's Tower Web
# Connect the tablet to the WiFi network and and connect to the drone over UDP
# Run "tower udpin:127.0.0.1:1549" from the terminal
# Run "python wc1.py --connect 127.0.0.1:14550"

# To begin real vehicle testing with DroneKit Python running on the computer
# Connect to Solo's WiFi network
# mavproxy.py --master=udpin:0.0.0.0:14550 --out=udpout:10.1.49.130:14550 --out=udpout:127.0.0.1:14550 --out=udpout:127.0.0.1:1549
# I HAVE NO IDEA WHY SOLO'S IP IS 0.0.0.0:14550 and not 10.1.1.1
# The first UDP out is for an my Android tablet so I can use Tower, the second is this Python script, and the third is for Tim's Tower Web
# Connect the tablet to the WiFi network and and connect to the drone over UDP
# Run "tower udpin:127.0.0.1:1549" from the terminal
# Run "python wc1.py --connect 127.0.0.1:14550"