########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
include "utils/vec2.bzz"
include "act/barrier.bzz"
include "utils/conversions.bzz"
include "utils/quickhull.bzz"
include "act/naviguation.bzz"
include "act/CA.bzz"
include "act/neighborcomm.bzz"

TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
WP_STIG = 8

path_it = 0
pic_time = 0
g_it = 0
homegps = {}

# Core state function when on the ground
function turnedoff() {
    BVMSTATE = "TURNEDOFF"
}

# Core state function when hovering
function idle() {
    BVMSTATE = "IDLE"
}

# Core state function to launch the robot: takeoff and wait for others, or stop (land)
function launch() {
	BVMSTATE = "LAUNCH"
	neighbors.broadcast("cmd", 22)
	log("AUTO_LAUNCH_STATE: ", AUTO_LAUNCH_STATE)
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
		homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
		log("Recorded home point: ",homegps.lat, homegps.lng)
		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
			barrier_ready(22)
		} else {
			log("Altitude: ", pose.position.altitude)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
		barrier_ready(22)
	}
}

# Launch function version without the timeout and stop state.
function launch_switch() {
	BVMSTATE = "LAUNCH_SWITCH"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
		homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
			barrier_ready(22)
		} else {
			log("Altitude: ", pose.position.altitude)
			neighbors.broadcast("cmd", 22)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
		barrier_ready(22)
	}
}

gohomeT=100
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
function goinghome() {
  BVMSTATE = "GOHOME"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
		m_navigation = vec_from_gps(homegps.lat, homegps.lng, 0)
		#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
		if(math.vec2.length(m_navigation) < GOTODIST_TOL){ # reached destination
				BVMSTATE = AUTO_LAUNCH_STATE
		} else {
			m_navigation = LimitSpeed(m_navigation, 3.0)
			#m_navigation = LCA(m_navigation)
			goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
		}
	} else
		BVMSTATE = AUTO_LAUNCH_STATE
}

# Core state function to stop and land.
function stop() {
  BVMSTATE = "STOP"
  neighbors.broadcast("cmd", 21)
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		uav_land()
		if(pose.position.altitude <= 0.2) {
			BVMSTATE = "STOP"
			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
			barrier_ready(21)
		}
	} else {
			BVMSTATE = "STOP"
			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
			barrier_ready(21)
	}
}

# State functions for individual waypoint control

wpreached = 1
function indiWP() {
	BVMSTATE = "WAYPOINT"
	check_rc_wp()

	wpi = v_wp.get(id)
	if(wpi!=nil) {
		wp = unpackWP2i(wpi)
		if(wp.pro == 0) {
				wpreached = 0
				storegoal(wp.lat, wp.lng, pose.position.altitude)
				var ls = packWP2i(wp.lat, wp.lng, 1)
				v_wp.put(id,ls)
				return
			}      
	}

	if(wpreached!=1)
		goto_gps(indiWP_done)

}

function indiWP_done() {
	BVMSTATE = "WAYPOINT"
	wpreached = 1
}

# State function to take a picture from the camera server (developed by HS)
function take_picture() {
  BVMSTATE="PICTURE"
  setgimbal(0.0, 0.0, -90.0, 20.0)
	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
	  takepicture()
  } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
    BVMSTATE="IDLE"
    pic_time=0
  }
  pic_time=pic_time+1
}

# State function to follow a moving attractor (GPS sent from a user phone)
function follow() {
	if(size(targets)>0) {
		BVMSTATE = "FOLLOW"
		attractor=math.vec2.newp(0,0)
		foreach(targets, function(id, tab) {
			force=(0.05)*(tab.range)^4
			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
		})
		goto_abs(attractor.x, attractor.y, 0.0, 0.0)
	} else {
		log("No target in local table!")
		BVMSTATE = "IDLE"
	}
}

# State function to converge to centroid
function aggregate() {
  BVMSTATE="AGGREGATE"
  centroid = neighbors.reduce(function(rid, data, centroid) {
	  centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
	  return centroid
  }, {.x=0, .y=0})
  if(neighbors.count() > 0)
    centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
	cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
  cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
  goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
}

# State fucntion to circle all together around centroid
function pursuit() {
  BVMSTATE="PURSUIT"
	rd = 20.0
	pc = 3.2
	vd = 15.0
  r_vec = neighbors.reduce(function(rid, data, r_vec) {
	  r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
	  return r_vec
  }, {.x=0, .y=0})
  if(neighbors.count() > 0)
    r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
	r = math.vec2.length(r_vec)
	var sqr = (r-rd)*(r-rd)+pc*pc*r*r
    gamma = vd / math.sqrt(sqr)
	dr = -gamma * (r-rd)
	dT = gamma * pc
  vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
  vfg = LimitSpeed(vfg, 2.0)
  goto_abs(vfg.x, vfg.y, 0.0, 0.0)
}

# Lennard-Jones interaction magnitude
TARGET     = 8.0
EPSILON    = 30.0 #30
GAIN_ATT	 = 50.0
GAIN_REP	 = 30.0
function lj_magnitude(dist, target, epsilon) {
	lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) #repulse only to avoid each other
  return lj
}

# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}

# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
  return math.vec2.add(data, accum)
}

# State function that calculates and actuates LJ flocking interaction with vstig targets (attractor/repulsors)
function lennardjones() {
  BVMSTATE="POTENTIAL"
	check_rc_wp()
	if(V_TYPE == 2)		# NOT MOVING!
		return
  # Calculate accumulator
  accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
  if(neighbors.count() > 0)
    accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
	# Add attractor/repulsor effects
	log(v_wp.size())
	accum_t = math.vec2.new(0.0, 0.0);
	v_wp.foreach(
    function(key, value, robot){
			wp = unpackWP2i(value)
			dvec = vec_from_gps(wp.lat, wp.lon, 0)
			Tdist = math.vec2.length(dvec)
			if(key > 99 and Tdist < 40)
				accum_t = math.vec2.sub(accum_t, math.vec2.newp(GAIN_REP*(TARGET/Tdist)^4, math.vec2.angle(dvec)))
			else if(key > 49 and Tdist > 10)
				accum_t = math.vec2.add(accum_t, math.vec2.newp(GAIN_ATT*(TARGET/Tdist)^2, math.vec2.angle(dvec)))
	})
	if(v_wp.size() > 0)
    accum_t = math.vec2.scale(accum_t, 1.0 / v_wp.size())
	#log(math.vec2.length(accum_t),math.vec2.length(accum_lj))
	
	accum_lj = LimitSpeed(math.vec2.add(accum_t,accum_lj), 1.0) #1/3
  goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}

# State function that calculates and actuates Voronoi Centroidal tessellation coverage (attractor/repulsors)
counter = 0
function voronoicentroid() {
  BVMSTATE="DEPLOY"
	check_rc_wp()
	wptab = v_wp.get(WPtab_id)
	if(wptab==nil)
		return
	else if(not(size(wptab) > 0))
		return
	log("WP table size:", size(wptab))
	if(V_TYPE == 2)
		return
	it_pts = 0
	att = {}
	foreach(wptab, function(key, value){
			wp = unpackWP2i(value)
			if(key > 99)
				log("Nothing planed for the repulsors yet....")
			else if(key > 49)
				att[it_pts]=vec_from_gps(wp.lat, wp.lng, 0)
			it_pts = it_pts + 1
	})
	# Boundaries from Geofence
	#it_pts = 0
	#foreach(GPSlimit, function(key, value) {
	#		bounds[it_pts]=vec_from_gps(value.lat, value.lng, 0)
	#		it_pts = it_pts + 1
	#})
	# Boundaries from user attractors
	#att = {.0=vec_from_gps(45.510283, -73.609633, 0), .1=vec_from_gps(45.510398, -73.609281, 0)}
	bounds = QuickHull(att)
	if(size(bounds)<3 )
		return
	if(counter==0) {
		pts = {.np=size(bounds)}
		it_pts = 0
		foreach(bounds, function(key, value) {
			pts[it_pts]=value
			it_pts = it_pts + 1
		})
		pts[it_pts] = {.x=0, .y=0}	#add itself
		it_pts = it_pts + 1
		if(neighbors.count() > 0) {
			neighbors.foreach(function(rid, data) {
				if(rid!=0){	#remove GS (?)
					pts[it_pts]=math.vec2.newp(data.distance,data.azimuth)
					it_pts = it_pts + 1
				}
			})
			#table_print(pts)
			voronoi(pts)
		}
		counter=4
	}
	counter=counter-1

	goto_gps(voronoicentroid_done)
}

function voronoicentroid_done() {
  BVMSTATE="DEPLOY"
}

# Custom state function for the developer to play with
function cusfun(){
	BVMSTATE="CUSFUN"
	#log("cusfun: yay!!!")
	#LOCAL_TARGET = math.vec2.new(5 ,0 )
	#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
    #if(math.vec2.length(local_set_point) > GOTODIST_TOL){
	#	local_set_point = LimitSpeed(local_set_point, 1.0)
	#	goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
    #}
    #else{
    #	log("TARGET REACHED")
    #}

	initialPoint = 0

	# Get waypoint list
	if (id == 1) {
		waypoints = {
			.0 = math.vec2.new(-5.0, -5.0),
			.1 = math.vec2.new(5.0, 5.0)
		}
	} else if (id == 2 ) {
		waypoints = {
			.0 = math.vec2.new(5.0, 5.0),
			.1 = math.vec2.new(-5.0, -5.0)
		}
	}

	# Current waypoint
	target_point = waypoints[point]

	# Get drone position and transform to global frame - depends on lauch file
	offset_x = 0.0
	offset_y = 0.0
	if (id == 1) {
		offset_x = 5.0
		offset_y = 5.0
	} else if (id == 2 ) {
		offset_x = -5.0
		offset_y = -5.0
	}
	pos_x = pose.position.x + offset_x;
	pos_y = pose.position.y + offset_y;

	# Get a vector pointing to target and target distance
	vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
	distance_to_target = math.vec2.length(vector_to_target)

	# Run LCA and increment waypoint
	if(distance_to_target > 0.2){
		vector_to_target = LCA(vector_to_target)
		vector_to_target = LimitSpeed(vector_to_target, 0.5)
		goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
	} else {
		if(point < size(waypoints) - 1){
			point = point + 1
		} else {
			point = 0
		}
	}
}