#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz"	#don't use a stigmergy id=11 with this header.
include "uavstates.bzz"

#
#Constant parameters, need to be adjust
#parameters for set_wheels
m_sWheelTurningParams={.MaxSpeed=1.0}

ROBOT_RADIUS=0.5
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER

#
# Global variables
#

#
#Save message from all neighours
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageSide={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCunt=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing
m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}

#Current robot state
m_eState="STATE_FREE"

#navigation vector
m_navigation={.x=0,.y=0}

#Debug message to be displayed in qt-opengl
#m_ossDebugMsg

#Debug vector to draw
#CVector2 m_cDebugVector

#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}

#Current label being requested or chosen (-1 when none)
m_nLabel=-1

#Label request id
m_unRequestId=0

#Side
m_unbSide=0

#Global bias, used to map local coordinate to global coordinate
m_bias=0

#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}

#Counter to wait for something to happen
m_unWaitCount=0

#Number of steps to wait before looking for a free label
m_unLabelSearchWaitTime=0

#Number of steps to wait for an answer to be received
m_unResponseTimeThreshold=0

#Number of steps to wait until giving up joining
m_unJoiningLostPeriod=0

#Tolerance distance to a target location
m_fTargetDistanceTolerance=0

#step cunt
step_cunt=0

#virtual stigmergy
v_tag = stigmergy.create(1)

# Lennard-Jones parameters
EPSILON    = 3.5 #3.5

# Lennard-Jones interaction magnitude

function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}

function Norm(vector) {
  var l = math.vec2.length(vector)
  return {
    .x = vector.x / l,
    .y = vector.y / l
  }
}

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
}

#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
Length = function(v) {
  return math.sqrt(v.x * v.x + v.y * v.y)
}

#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
  return math.atan(v.y, v.x)
}

#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}

#
#return the index of value
#
function find(table,value){
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
}

function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
    while(i<exponent){
    renturn_val=renturn_val*base
    i=i+1
    }
    return renturn_val
    }
}

#
# Graph parsing
#
function parse_graph(fname) {
	# Graph data
	var gd = {}
	# Open the file
	var fd = io.fopen(fname, "r")
	if(not fd) {
		log("Can't open '", fname, "'")
		return nil
	}
	# Parse the file line by line
	var rrec # Record read from line
	var grec # Record parsed into graph
	io.fforeach(fd, function(line) {
		  # Parse file line
		  rrec = string.split(line, "\t ")
		  # Make record
		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
			  .Lable = string.toint(rrec[0]),   # Lable of the point
			  .Pred = string.toint(rrec[1]),   # Lable of its predecessor
			  .distance = string.tofloat(rrec[2]), # distance to the predecessor
			  .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
			  .State="UNASSIGNED",
              .StateAge=0
		  }})
	# All done
	io.fclose(fd)
	return gd
}

function parse_graph_fixed(fname) {
	# Graph data
	var gd = {}
	# Open the file
	var fd = io.fopen(fname, "r")
	if(not fd) {
		log("Can't open '", fname, "'")
		return nil
	}
	# Parse the file line by line
	var rrec # Record read from line
	var grec # Record parsed into graph
	io.fforeach(fd, function(line) {
		  # Parse file line
		  rrec = string.split(line, "\t ")
		  # Make record
		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2, side
			  .Pred1 = string.toint(rrec[1]),   # Pred 1 lable
			  .Pred2 = string.toint(rrec[3]),   # Pred 2 lable
			  .d1 = string.tofloat(rrec[2]), # Pred 1 distance
			  .d2 = string.tofloat(rrec[4]), # Pred 2 distance
			  #.S  = string.toint(rrec[5]),   # Side
			  .Lable=string.toint(rrec[0]),
			  .State="UNASSIGNED",
              .StateAge=0              
		  }})
	# All done
	io.fclose(fd)
	return gd
}


function start_listen(){
neighbors.listen("msg",
    function(vid,value,rid){
    #store the received message
    var temp_id=rid
    m_receivedMessage.State=value.State
    m_receivedMessage.Lable=value.Lable
    m_receivedMessage.ReqLable=value.ReqLable
    m_receivedMessage.ReqID=value.ReqID
    m_receivedMessage.Side=value.Side
    m_receivedMessage.Response=value.Response
    Get_DisAndAzi(temp_id)
    #add the received message
    #
    m_MessageState[m_neighbourCunt]=value.State
    m_MessageLable[m_neighbourCunt]=value.Lable
    m_MessageReqLable[m_neighbourCunt]=value.ReqLable
    m_MessageReqID[m_neighbourCunt]=value.ReqID
    m_MessageSide[m_neighbourCunt]=value.Side
    m_MessageResponse[m_neighbourCunt]=value.Response
    m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
    m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
    m_neighbourCunt=m_neighbourCunt+1
})
}
#
#Function used to get the station info of the sender of the message
function Get_DisAndAzi(id){
neighbors.foreach(
  function(rid, data) {
	if(rid==id){
    m_receivedMessage.Range=data.distance
    m_receivedMessage.Bearing=data.azimuth
    }
    })
}

#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0

while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
	m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
	m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
	m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
	m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
	if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
	m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
	if(m_vecNodes[i].StateAge==0)
		m_vecNodes[i].State="UNASSIGNED"
	}
i=i+1
}
}

#
#Transistion to state free
#
function TransitionToFree(){
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=m_eState
}

#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId

m_unWaitCount=m_unResponseTimeThreshold
}

#
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=m_eState 
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod

neighbors.listen("reply",
    function(vid,value,rid){
    #store the received message
    if(value.Lable==m_nLabel){
	m_cMeToPred.GlobalBearing=value.GlobalBearing

	}
})

}

#
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("reply")

#write statues
v_tag.put(m_nLabel, 1)

m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
}

#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"

m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
}

#
# Do free
#
function DoFree() {
	m_selfMessage.State=m_eState
	#wait for a while before looking for a lable
	if(m_unWaitCount>0)
		m_unWaitCount=m_unWaitCount-1

	#find a set of joined robots
	var setJoinedLables={}
	var setJoinedIndexes={}
	var i=0
	var j=0
	while(i<m_neighbourCunt){
		if(m_MessageState[i]=="STATE_JOINED"){
		setJoinedLables[j]=m_MessageLable[i]
		setJoinedIndexes[j]=i
		j=j+1
		}
		i=i+1
	}

	#go through the graph to look for a proper lable
	var unFoundLable=0
	var IDofPred=0
	i=1
	while(i<size(m_vecNodes) and (unFoundLable==0)){
		#if the node is unassigned and the predecessor is insight
		if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
			unFoundLable=m_vecNodes[i].Lable 
			IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
		}
		i=i+1
	}

	if(unFoundLable>0){
		TransitionToAsking(unFoundLable)
		return
	}

	#navigation
	#if there is a joined robot within sight, move around joined robots
	#else, gather with other free robots
	if(size(setJoinedIndexes)>0){
		var tempvec_P={.x=0.0,.y=0.0}
		var tempvec_N={.x=0.0,.y=0.0}
		i=0
		while(i<size(setJoinedIndexes)){
			var index=setJoinedIndexes[i]
			tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
			tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
			i=i+1
		}
		tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
		tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
		m_navigation=math.vec2.add(tempvec_P,tempvec_N)
		uav_moveto(m_navigation.x,m_navigation.y)
	}else{ #no joined robots in sight
		i=0
		var tempvec={.x=0.0,.y=0.0}

		while(i<m_neighbourCunt){
			tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
			i=i+1
		}
		m_navigation=math.vec2.scale(tempvec,1.0/i)
		uav_moveto(m_navigation.x,m_navigation.y)
	}

	#collision avoidence
	i=0
	var turnAngle=0.0
	var needHide=0
	while(i<m_neighbourCunt){
		#if there is a robot within tolerance before, turn 90 degree to hide
		if(m_MessageRange[i]<ROBOT_SAFETYDIST){
			turnAngle=m_MessageBearing[i]+math.pi/2.0
			needHide=1
		}
		i=i+1
	}

	if(needHide==1){
		m_navigation.x=0.0
		m_navigation.y=0.0
		m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
		uav_moveto(m_navigation.x,m_navigation.y)
	}

	#jump the first step
	if(step_cunt<=1){
		uav_moveto(0.0,0.0)
	}
	#set message
	m_selfMessage.State=m_eState
}




#
#Do asking
#
function DoAsking(){
	#look for response from predecessor
	var i=0
	var psResponse=-1
	while(i<m_neighbourCunt and psResponse==-1){
		#the respond robot in joined state
		#the request lable be the same as requesed
		#get a respond
		if(m_MessageState[i]=="STATE_JOINED"){
			if(m_MessageReqLable[i]==m_nLabel)
				if(m_MessageResponse[i]!="REQ_NONE"){
				psResponse=i
		}}
		i=i+1
	}
	#analyse response
	if(psResponse==-1){
		#no response, wait
		m_unWaitCount=m_unWaitCount-1
		m_selfMessage.State=m_eState
		m_selfMessage.ReqLable=m_nLabel
		m_selfMessage.ReqID=m_unRequestId
		if(m_unWaitCount==0){
			TransitionToFree()
			return
		}
	}
	else{
	if(m_MessageReqID[psResponse]!=m_unRequestId)
		TransitionToFree()
	if(m_MessageReqID[psResponse]==m_unRequestId){
		if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
			TransitionToJoining()
			#TransitionToJoined()
			return
		}
		else{
			TransitionToAsking(m_nLabel)
			return
		}
	}
	}
	uav_moveto(0.0,0.0)
}

#
#Do joining
#
function DoJoining(){
	#get information of pred
	var i=0
	var IDofPred=-1
	while(i<m_neighbourCunt and IDofPred==-1){
		if(m_MessageLable[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
			IDofPred=i
		i=i+1
	}

	#found pred
	if(IDofPred!=-1){
		m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer

		var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
		m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
		m_cMeToPred.Bearing=m_MessageBearing[IDofPred]

		#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
		var S2PGlobalBearing=0

		#m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)

		if(m_cMeToPred.GlobalBearing>math.pi)
			S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
		else
			S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi

		var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)

		#the vector from self to target in global coordinate
		var S2Target=math.vec2.add(S2Pred,P2Target)
		#change the vector to local coordinate of self
		var S2Target_dis=Length(S2Target)
		var S2Target_bearing=Angle(S2Target)
		m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
		S2Target_bearing=S2Target_bearing+m_bias
		m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
		uav_moveto(m_navigation.x,m_navigation.y)


		#test if is already in desired position
		if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
			TransitionToJoined()
			return
		}
	} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
		m_unWaitCount=m_unWaitCount-1
	}

	#avoide collision
	i=0
	var turnAngle=0.0
	var needHide=0
	while(i<m_neighbourCunt){
		#if there is a robot within tolerance before, turn 90 degree to hide
		if(m_MessageRange[i]<ROBOT_SAFETYDIST){
			turnAngle=m_MessageBearing[i]+math.pi/2.0
			needHide=1
		}
		i=i+1
	}

	if(needHide==1){
		m_navigation.x=0.0
		m_navigation.y=0.0
		m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
		uav_moveto(m_navigation.x,m_navigation.y)
	}
	if(m_unWaitCount==0){
		TransitionToFree()
		return
	}

	#pack the communication package
	m_selfMessage.State=m_eState
	m_selfMessage.Lable=m_nLabel

}

#
#Do joined
#
function DoJoined(){
	m_selfMessage.State=m_eState
	m_selfMessage.Lable=m_nLabel

	#collect all requests
	var mapRequests={}
	var i=0
	var j=0
	var ReqLable
	var JoiningLable
	var seenPred=0
	while(i<m_neighbourCunt){
		if(m_MessageState[i]=="STATE_ASKING"){
			ReqLable=m_MessageReqLable[i]
			if(m_vecNodes[ReqLable].State=="UNASSIGNED")
				if(m_nLabel==m_vecNodes[ReqLable].Pred){
					#is a request, store the index
					mapRequests[j]=i
					j=j+1
				}
		}
		if(m_MessageState[i]=="STATE_JOINING"){
			JoiningLable=m_MessageLable[i]
			if(m_nLabel==m_vecNodes[JoiningLable].Pred){
				##joining wrt this dot,send the global bearing
				var m_messageForJoining={.Lable=JoiningLable,.GlobalBearing=m_MessageBearing[i]-m_bias}
				neighbors.broadcast("reply",m_messageForJoining)
			}
		}
		#if it is the pred
		if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[i]==m_vecNodes[m_nLabel].Pred){
			seenPred=1
			m_unWaitCount=m_unJoiningLostPeriod 
		}
		i=i+1
	}

	#get request
	if(size(mapRequests)!=0){
		i=1
		var ReqIndex=0
		while(i<size(mapRequests)){
			#compare the distance
			if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
				ReqIndex=i
			i=i+1
		}
		#get the best index, whose Reqlable and Reqid are
		ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
		var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
		m_selfMessage.ReqLable=ReqLable
		m_selfMessage.ReqID=ReqID
		m_selfMessage.Response="REQ_GRANTED"
	}

	#lost pred, wait for some time and transit to free
	#if(seenPred==0){
	#m_unWaitCount=m_unWaitCount-1
	#if(m_unWaitCount==0){
	#TransitionToFree()
	#return 
	#}
	#}


	m_navigation.x=0.0
	m_navigation.y=0.0
	uav_moveto(m_navigation.x,m_navigation.y)


	#check if should to transists to lock


	if(v_tag.size()==ROBOTS){
		TransitionToLock()
	}
}

#
#Do Lock
#
function DoLock(){
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel

m_navigation.x=0.0
m_navigation.y=0.0

#collect preds information
var i=0
var mypred1={.range=0,.bearing=0}
var mypred2={.range=0,.bearing=0}

while(i<m_neighbourCunt){

	#is the first predecessor
	if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred1){
		mypred1.range=m_MessageRange[i]
		mypred1.bearing=m_MessageBearing[i]
	}
	#is the second predecessor
	if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred2){
		mypred2.range=m_MessageRange[i]
		mypred2.bearing=m_MessageBearing[i]
	}
i=i+1
}

#calculate motion vection
if(m_nLabel==0){
	m_navigation.x=0.0
	m_navigation.y=0.0
	log(";",m_nLabel,";",0)
}

if(m_nLabel==1){
	var tempvec={.Range=0.0,.Bearing=0.0}
	tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,10)
	#tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1
	tempvec.Bearing=mypred1.bearing
	m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing)
	log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
if(m_nLabel>1){
	var cDir={.x=0.0,.y=0.0}
	var cDir1={.x=0.0,.y=0.0}
	var cDir2={.x=0.0,.y=0.0}
	cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing)	
	cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing)
	#cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing)	
	#cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing)
	cDir=math.vec2.add(cDir1,cDir2)

	cDir=math.vec2.scale(cDir,100)
	m_navigation.x=cDir.x
	m_navigation.y=cDir.y
	#log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2)
	log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x,m_navigation.y)
}

#
# Executed at init
#
function init() {
	#
	#Adjust parameters here
	#
	m_unResponseTimeThreshold=10
	m_unLabelSearchWaitTime=10
	m_fTargetDistanceTolerance=1.5
	m_unJoiningLostPeriod=100

	#
	# Join Swarm
	#
	s = swarm.create(1)
	s.join()
	#ROBOT_NUM=5
	Reset();
	statef=turnedoff
}

#
# Executed every step
#
function step(){
	uavcmd()
	#update the graph
	UpdateNodeInfo()
	#reset message package to be sent
	m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
	#
	#act according to current state
	#
	if(UAVSTATE=="IDLE"){
		if(m_eState=="STATE_FREE")
			DoFree()
		else if(m_eState=="STATE_ESCAPE")
			DoEscape()
		else if(m_eState=="STATE_ASKING")
			DoAsking()
		else if(m_eState=="STATE_JOINING")
			DoJoining()
		else if(m_eState=="STATE_JOINED")
			DoJoined()
		else if(m_eState=="STATE_LOCK")
			DoLock()
	}

	statef()


	debug(m_eState,m_nLabel)
	log("Current state: ", UAVSTATE)
	log("Swarm size: ", ROBOTS)
	#navigation

	#broadcast messag
	neighbors.broadcast("msg",m_selfMessage)

	#
	#clean message storage
	m_MessageState={}#store received neighbour message
	m_MessageLable={}#store received neighbour message
	m_MessageReqLable={}#store received neighbour message
	m_MessageReqID={}#store received neighbour message
	m_MessageSide={}#store received neighbour message
	m_MessageResponse={}#store received neighbour message
	m_MessageRange={}#store received neighbour message
	m_MessageBearing={}#store received neighbour message
	m_neighbourCunt=0


	#step cunt+1
	step_cunt=step_cunt+1
}

#
# Executed when reset
#
function Reset(){
    m_vecNodes={}
    m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/script/Graph_drone.graph")#change the .graph file when necessary
    m_vecNodes_fixed={}
    m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/script/Graph_fixed.graph")
    m_nLabel=-1

	#start listening
	start_listen()
	#
	#set initial state, only one robot choose [A], while the rest choose [B]
	#
	#[A]The robot used to triger the formation process is defined as joined,
	if(id==0){
		m_nLabel=0
		TransitionToJoined()
	}
	#[B]Other robots are defined as free.
	else{
		TransitionToFree()
	}
}

#
# Executed upon destroy
#
function destroy() {
	#clear neighbour message
	uav_moveto(0.0,0.0)
	m_vecNodes={}
	#stop listening 
	neighbors.ignore("msg")
}