ROSBuzz_MISTLab/buzz_scripts/include/uavstates.bzz

########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
TARGET_ALTITUDE = 5.0 # m.
UAVSTATE = "TURNEDOFF"
PICTURE_WAIT = 40 # steps
GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5  # m.
GOTOANG_TOL = 0.1 # rad.
goal = {.range=0, .bearing=0, .latitude=0, .longitude=0}

function uav_initswarm() {
	s = swarm.create(1)
	s.join()
}

function turnedoff() {
    statef=turnedoff
    UAVSTATE = "TURNEDOFF"
}

function idle() {
    statef=idle
    UAVSTATE = "IDLE"
}

function takeoff() {
	UAVSTATE = "TAKEOFF"
	statef=takeoff
	
	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
		barrier_set(ROBOTS, action, land, -1)
		barrier_ready()
	} else {
    log("Altitude: ", TARGET_ALTITUDE)
	  neighbors.broadcast("cmd", 22)
	  uav_takeoff(TARGET_ALTITUDE)
  }
}

function land() {
	UAVSTATE = "LAND"
	statef=land

	neighbors.broadcast("cmd", 21)
	uav_land()
	
	if(flight.status != 2 and flight.status != 3){
    barrier_set(ROBOTS,turnedoff,land, 21)
		barrier_ready()
	}
}

function set_goto(transf) {
	UAVSTATE = "GOTO"
	statef=function() {
    gotoWP(transf);
  }

	if(rc_goto.id==id){
		if(s!=nil){
      if(s.in())
        s.leave()      
    }
	} else {
		neighbors.broadcast("cmd", 16)
		neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
	}
}

ptime=0
function picture() {
  statef=picture
  UAVSTATE="PICTURE"
	#TODO: change gimbal orientation
	if(ptime==PICTURE_WAIT/2)
	  uav_takepicture()
  else if(ptime>=PICTURE_WAIT) {
    statef=action
    ptime=0 
  }
  ptime=ptime+1
}

function gotoWP(transf) {
	# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
	rb_from_gps(rc_goto.latitude, rc_goto.longitude)
	print(" has to move ", goal.range)
	m_navigation = math.vec2.newp(goal.range, goal.bearing)

	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
    log("Sorry this is too far.")
  else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
		transf()
	else
		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
}

function follow() {
	if(size(targets)>0) {
		UAVSTATE = "FOLLOW"
		statef=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))
		})
		uav_moveto(attractor.x, attractor.y, 0.0)
	} else {
		log("No target in local table!")
		#statef=idle
	}
}

function uav_rccmd() {
    if(flight.rc_cmd==22) {
		log("cmd 22")
		flight.rc_cmd=0
		statef = takeoff
		UAVSTATE = "TAKEOFF"		
		neighbors.broadcast("cmd", 22)
	} else if(flight.rc_cmd==21) {
		log("cmd 21")
		log("To land")
		flight.rc_cmd=0
		statef = land
		UAVSTATE = "LAND"
		neighbors.broadcast("cmd", 21)
	} else if(flight.rc_cmd==16) {
		flight.rc_cmd=0
		UAVSTATE = "GOTO"
		statef = goto
	} else if(flight.rc_cmd==400) {
		flight.rc_cmd=0
		uav_arm()
		neighbors.broadcast("cmd", 400)
	} else if (flight.rc_cmd==401){
		flight.rc_cmd=0
		uav_disarm()
		neighbors.broadcast("cmd", 401)
	} else if (flight.rc_cmd==666){
		flight.rc_cmd=0
		stattab_send()
	}
}

function uav_neicmd() {
    neighbors.listen("cmd",
			function(vid, value, rid) {
				print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")")
				if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
					statef=takeoff
		      UAVSTATE = "TAKEOFF"
				} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
					statef=land
		      UAVSTATE = "LAND"
				} else if(value==400 and UAVSTATE=="TURNEDOFF") {
					uav_arm()
				} else if(value==401 and UAVSTATE=="TURNEDOFF"){
					uav_disarm()
				} else if(value==16 and UAVSTATE=="IDLE"){
				#  neighbors.listen("gt",function(vid, value, rid) {
				#    print("Got (", vid, ",", value, ") from robot #", rid)
				#    # if(gt.id == id) statef=goto
				#  }) 
				}
    })
}

function LimitAngle(angle){
	if(angle>2*math.pi)
		return angle-2*math.pi
	else if (angle<0)
		return angle+2*math.pi
	else
		return angle
}

function rb_from_gps(lat, lon) {
#  print("gps2rb: ",lat,lon,position.latitude,position.longitude)
	d_lon = lon - position.longitude
  d_lat = lat - position.latitude
  ned_x = d_lat/180*math.pi * 6371000.0;
  ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
	goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
	goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
}

function gps_from_vec(vec) {
  Vrange = math.vec2.length(vec)
  Vbearing = LimitAngle(math.atan(vec.y, vec.x))
#  print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
  latR = position.latitude*math.pi/180.0;
  lonR = position.longitude*math.pi/180.0;
  target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
  target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
  goal.latitude = target_lat*180.0/math.pi;
  goal.longitude = target_lon*180.0/math.pi;
  #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
  #goal.latitude = d_lat + position.latitude;
  #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
  #goal.longitude = d_lon + position.longitude;
}