#!/usr/bin/env python

import time
import subprocess
import argparse
import os
import signal
from dronekit import VehicleMode

from drone import debug, Drone
import drone as dronelib



def shellCmdWait(cmd):
	subprocess.call(cmd, shell=True)

def shellCmdNoWait(cmd):
	return subprocess.Popen(cmd, shell=True, preexec_fn=os.setpgrp)



# Set up option parsing to get connection string
parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.')
parser.add_argument('--connect',
                    help="Vehicle connection target string. Required.")

parser.add_argument('--date1',
                    help="Current date, used for directiory mapping. Required.")

parser.add_argument('--date2',
                    help="Current time, used for directiory mapping. Required.")

args = parser.parse_args()

connectionString = args.connect

date1 = args.date1

date2 = args.date2

dronelib.setupDebug(date1, date2)

if not connectionString:
	print("Connection argument required.")
	exit()

stream = None
drone = None

try:
	# Connect to the Vehicle
	debug('Connecting to vehicle on: %s' % connectionString)
	drone = Drone(connectionString)

	drone.waitForArm()

	cmd = "sudo arp-scan --interface=wlx00e04c1cd65f --localnet --quiet > tempout.txt"
	shellCmdWait(cmd)

	file = open("tempout.txt", "r")

	lines = file.readlines()

	s = sets.Set()

	for i in xrange(2,len(lines) - 3):
		if lines[i] == "\n":
			break
		s.add(lines[i].split("\t")[0])

	length = len(s)
	clients = ""

	for i in xrange(0,length):
		if i == length - 1:
			clients = clients + s.pop() + ":5600"
		else:
			clients = clients + s.pop() + ":5600,"

	# open video stream
	command = "raspivid -t 0 -h 1080 -w 1920 -fps 24 -hf -b 2000000 -o - | tee myvid.h264 | gst-launch-1.0 -v fdsrc ! h264parse ! rtph264pay config-interval=1 pt=96 ! multiudpsink clients=" + clients
	stream = shellCmdNoWait(command)

	time.sleep(3)

	debug("Set default/target airspeed to 10")
	drone.vehicle.airspeed = 10

	for i in xrange(0,20):
		drone.send_global_velocity(0,0,-10)
		time.sleep(1)

	time.sleep(3)

except Exception,e:
	debug(repr(e))

finally:

	time.sleep(2)

	debug("Kill stream & subprocesses")

	pgrp = os.getpgid(stream.pid)
	os.killpg(pgrp, signal.SIGKILL)

	time.sleep(2)

	debug("Convert video")
	command2 = "sudo MP4Box -fps 24 -add myvid.h264 myvid.mp4"
	shellCmdWait(command2)

	debug("Cleaning up")
	command3 = "sudo rm *.h264"
	shellCmdWait(command3)

	command4 = "sudo mv myvid.mp4 /home/logs/copter/logs" + date1 + "Mission" + date1 + "_" + date2 +"/flight1/video.mp4"
	shellCmdWait(command4)

	debug("Close vehicle object")
	drone = drone.close()

	debug("Closing log")
	dronelib.closeDebug()