#
# Include files
#

include "taskallocate/graphs/shapes_P.bzz"
include "taskallocate/graphs/shapes_O.bzz"
include "taskallocate/graphs/shapes_L.bzz"
include "taskallocate/graphs/shapes_Y.bzz"

ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
graph_id = 3
graph_loop = 1

#
# 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_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}

#
#Save the message to send
#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}

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

#Current label being requested or chosen (-1 when none)
m_nLabel=-1
m_messageID={}
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}

#Label request id
m_unRequestId=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

# virtual stigmergy for the LOCK barrier.
m_lockstig = 1

# Lennard-Jones parameters, may need change
EPSILON_GRAPH	= 4000 #13.5 the LJ parameter for other robots

#
#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
}
#
#map from int to state
#
function i2s(value){
	if(value==1){
	return "GRAPH_FREE"
	}
	else if(value==2){
	return "GRAPH_ASKING"
	}
	else if(value==3){
	return "GRAPH_JOINING"
	}
	else if(value==4){
	return "GRAPH_JOINED"
	}
	else if(value==5){
	return "GRAPH_LOCK"
	}
}
#
#map from state to int
#
function s2i(value){
	if(value=="GRAPH_FREE"){
	return 1
	}
	else if(value=="GRAPH_ASKING"){
	return 2
	}
	else if(value=="GRAPH_JOINING"){
	return 3
	}
	else if(value=="GRAPH_JOINED"){
	return 4
	}
	else if(value=="GRAPH_LOCK"){
	return 5
	}
}
#
#map form int to response
#
function i2r(value){
	if(value==1){
	return "REQ_NONE"
	}
	else if(value==2){
	return "REQ_GRANTED"
	}
}
#
#map from response to int
#
function r2i(value){
	if(value=="REQ_NONE"){
	return 1
	}
	else if(value=="REQ_GRANTED"){
	return 2
	}
}
#
#return the index of value
#
function find(table,value){
	var ind=nil
	var i=0
	while(i<size(table)){
		if(table[i]==value)
			ind=i
		i=i+1
	}
	return ind
}
#
#pack message into 1 number
#
function packmessage(send_table){
    var send_value
    send_value=1000000*send_table.State+100000*send_table.Label+10000*send_table.ReqLabel+100*send_table.ReqID+send_table.Response
    return send_value
}
#
#pack guide message into 1 number
#
function pack_guide_msg(send_table){
    var send_value
    var r_id=send_table.Label#id of target robot
    var pon#positive or negative ,0 postive, 1 negative
    if(send_table.Bearing>=0){
    pon=0
    }
    else{
    pon=1
    }
    var b=math.abs(send_table.Bearing)
    send_value=r_id*1000+pon*100+b
    return send_value
}
#
#unpack message
#
function unpackmessage(recv_value){
    var wan=(recv_value-recv_value%1000000)/1000000
    recv_value=recv_value-wan*1000000
    var qian=(recv_value-recv_value%100000)/100000
    recv_value=recv_value-qian*100000
    var bai=(recv_value-recv_value%10000)/10000
    recv_value=recv_value-bai*10000
    var shi=(recv_value-recv_value%100)/100
    recv_value=recv_value-shi*100
    var ge=recv_value
    var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
    return_table.State=wan
    return_table.Label=qian
    return_table.ReqLabel=bai
    return_table.ReqID=shi
    return_table.Response=ge
    return return_table
}
#
#unpack guide message
#
function unpack_guide_msg(recv_value){
#log(id,"I pass value=",recv_value)
    var qian=(recv_value-recv_value%1000)/1000
    recv_value=recv_value-qian*1000
    var bai=(recv_value-recv_value%100)/100
    recv_value=recv_value-bai*100
    var b=recv_value
    var return_table={.Label=0.0,.Bearing=0.0}
    return_table.Label=qian
    if(bai==1){
    	b=b*-1.0
    }
    return_table.Bearing=b
    return return_table
}
#
#get the target distance to neighbr nei_id
#
function target4label(nei_id){
    var return_val="miss"
    var i=0
    while(i<size(lock_neighbor_id)){
		if(lock_neighbor_id[i]==nei_id){
			return_val=lock_neighbor_dis[i]
		}
		i=i+1
    }
    return return_val
}
#
#calculate LJ vector for neibhor stored in i
#
function LJ_vec(i){
    var dis=m_MessageRange[i]
    var ljbearing=m_MessageBearing[i]
    var nei_id=m_messageID[i]
    var target=target4label(nei_id)
    var cDir={.x=0.0,.y=0.0}
    if(target!="miss"){
    	cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing)
    }
    #log(id,"dis=",dis,"target=",target,"label=",nei_id)
		#log("x=",cDir.x,"y=",cDir.y)
    return cDir
}
#
#calculate the motion vector
#
function motion_vector(){
    var i=0
    var m_vector={.x=0.0,.y=0.0}
    while(i<m_neighbourCount){
		#calculate and add the motion vector
		m_vector=math.vec2.add(m_vector,LJ_vec(i))
		#log(id,"x=",m_vector.x,"y=",m_vector.y)
		i=i+1
    }
    m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
	#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
    return m_vector
}
#
# starts the neighbors listener
#
function start_listen(){
neighbors.listen("m",
    function(vid,value,rid){
    #store the received message
    var temp_id=rid
    var recv_val=unpackmessage(value)
    Get_DisAndAzi(temp_id)
    #add the received message
    #
    m_MessageState[m_neighbourCount]=i2s(recv_val.State)
    m_MessageLabel[m_neighbourCount]=recv_val.Label
    m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
    m_MessageReqID[m_neighbourCount]=recv_val.ReqID
    m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
    m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
    m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
	m_messageID[m_neighbourCount]=rid

#log(rid, " is in ",  m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])

    m_neighbourCount=m_neighbourCount+1
})
}
#
#Function used to get the station info of the sender of the message
#
function Get_DisAndAzi(t_id){
	neighbors.foreach(
	function(rid, data) {
		if(rid==t_id){
		m_receivedMessage.Range=data.distance*100.0
		m_receivedMessage.Bearing=data.azimuth
		}
		})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
	#Collect informaiton
	#Update information
	var i=0

	while(i<m_neighbourCount){
		if(m_MessageState[i]=="GRAPH_JOINED"){
			m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
			m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
		}
		else if(m_MessageState[i]=="GRAPH_JOINING"){
			m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
			m_vecNodes[m_MessageLabel[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
	}
}

#
# Do free
#
function DoFree() {
  if(BVMSTATE!="GRAPH_FREE"){
	BVMSTATE="GRAPH_FREE"
	m_unWaitCount=m_unLabelSearchWaitTime
	m_selfMessage.State=s2i(BVMSTATE)
  }else{
	UpdateGraph()
	m_selfMessage.State=s2i(BVMSTATE)

	#wait for a while before looking for a Label
	if(m_unWaitCount>0)
		m_unWaitCount=m_unWaitCount-1

	#find a set of joined robots
	var setJoinedLabels={}
	var setJoinedIndexes={}
	var i=0
	var j=0
	while(i<m_neighbourCount){
		if(m_MessageState[i]=="GRAPH_JOINED"){
		setJoinedLabels[j]=m_MessageLabel[i]
		setJoinedIndexes[j]=i
		j=j+1
		}
		i=i+1
	}

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

	if(unFoundLabel>0){
	    m_nLabel=unFoundLabel
		DoAsking()
		return
	}

	#set message
	m_selfMessage.State=s2i(BVMSTATE)

	BroadcastGraph()
  }
}
#
#Do asking
#
function DoAsking(){
	if(BVMSTATE!="GRAPH_ASKING"){
	  BVMSTATE="GRAPH_ASKING"
	  #math.rng.setseed(id)
	  m_unRequestId=id#math.rng.uniform(0,10)	#don't know why the random numbers are the same, add id to make the ReqID different
	  m_selfMessage.State=s2i(BVMSTATE)
	  m_selfMessage.ReqLabel=m_nLabel
	  m_selfMessage.ReqID=m_unRequestId
	  m_unWaitCount=m_unResponseTimeThreshold
	}else {
		UpdateGraph()
		#look for response from predecessor
		var i=0
		var psResponse=-1
		while(i<m_neighbourCount and psResponse==-1){
			#the respond robot in joined state
			#the request Label be the same as requesed
			#get a respond
			if(m_MessageState[i]=="GRAPH_JOINED"){
				#log("received label = ",m_MessageReqLabel[i])
				if(m_MessageReqLabel[i]==m_nLabel)
					if(m_MessageResponse[i]!="REQ_NONE"){
					psResponse=i
			}}
			if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
				DoFree()
				return
			}
			i=i+1
		}
		#analyse response
		if(psResponse==-1){
			#no response, wait

			m_unWaitCount=m_unWaitCount-1
			m_selfMessage.State=s2i(BVMSTATE)
			m_selfMessage.ReqLabel=m_nLabel
			m_selfMessage.ReqID=m_unRequestId
			#if(m_unWaitCount==0){
				#DoFree()
				#return
			#}
		}
		else{
			log("respond id=",m_MessageReqID[psResponse])
			if(m_MessageReqID[psResponse]!=m_unRequestId){
				m_vecNodes[m_nLabel].State="ASSIGNING"
				m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
				DoFree()
			}
		if(m_MessageReqID[psResponse]==m_unRequestId){
			if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
				DoJoining()
				return
			}
			else{
				DoAsking()
				return
			}
		}
		}
		m_selfMessage.Label=m_nLabel

		m_navigation.x=0.0
		m_navigation.y=0.0
		goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
		BroadcastGraph()
  }
}
#
#Do joining
#
function DoJoining(){
  if(BVMSTATE!="GRAPH_JOINING") {
	BVMSTATE="GRAPH_JOINING"
    m_selfMessage.State=s2i(BVMSTATE)
    m_selfMessage.Label=m_nLabel
	m_unWaitCount=m_unJoiningLostPeriod
  } else {
    UpdateGraph()
	if(m_gotjoinedparent!=1)
		set_rc_goto()
	else
		goto_gps(DoJoined)
		#pack the communication package
		m_selfMessage.State=s2i(BVMSTATE)
		m_selfMessage.Label=m_nLabel
		BroadcastGraph()
  }
}

function set_rc_goto() {
  #get information of pred
	var i=0
	var IDofPred=-1
	while(i<m_neighbourCount and IDofPred==-1){
		if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_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 position of self to pred in local coordinate
		m_cMeToPred.Bearing=m_MessageBearing[IDofPred]

		var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
		var S2Target=math.vec2.add(S2Pred,P2Target)

		goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
		print("Saving GPS goal (", m_nLabel, " from ", IDofPred, "): ",goal.latitude, goal.longitude, "(", S2Target.x/100, S2Target.y/100, ")")
		storegoal(goal.latitude, goal.longitude, pose.position.altitude)
		m_gotjoinedparent = 1
  	}
}
#
#Do joined
#
function DoJoined(){
	if(BVMSTATE!="GRAPH_JOINED"){
	  BVMSTATE="GRAPH_JOINED"
	  m_selfMessage.State=s2i(BVMSTATE)
	  m_selfMessage.Label=m_nLabel
	  m_vecNodes[m_nLabel].State="ASSIGNED"

	  #write statues
  	  barrier_create()
	  barrier_ready(-1)

	  m_navigation.x=0.0
	  m_navigation.y=0.0
	  goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
	} else {
		UpdateGraph()
		m_selfMessage.State=s2i(BVMSTATE)
		m_selfMessage.Label=m_nLabel

		#collect all requests
		var mapRequests={}
		var i=0
		var j=0
		var ReqLabel
		var JoiningLabel
		var seenPred=0
		while(i<m_neighbourCount){
			if(m_MessageState[i]=="GRAPH_ASKING"){
				ReqLabel=m_MessageReqLabel[i]
				#log("ReqLabel var:",ReqLabel)
				#log("M_vec var",m_vecNodes[ReqLabel].State)
				if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
					if(m_nLabel==m_vecNodes[ReqLabel].Pred){
						#is a request, store the index
						mapRequests[j]=i
						j=j+1
					}
			}
			if(m_MessageState[i]=="GRAPH_JOINING"){
				JoiningLabel=m_MessageLabel[i]
				if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
					##joining wrt this dot,send the global bearing
					var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
				}
			}

				if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
							repeat_assign=0
					}


			#if it is the pred
			if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[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
			}
			if(repeat_assign==0){
				#get the best index, whose ReqLabel and Reqid are
				ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
				var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
				assign_label=ReqLabel
				assign_id=ReqID
				repeat_assign=1
			}
			m_selfMessage.ReqLabel=assign_label
			m_selfMessage.ReqID=assign_id
			m_selfMessage.Response=r2i("REQ_GRANTED")
			#m_vecNodes[ReqLabel].State="ASSIGNING"
			log("Label=",assign_label)
			log("ID=",assign_id)
				m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
		}

		#lost pred, wait for some time and transit to free
		if(seenPred==0){
		m_unWaitCount=m_unWaitCount-1
		if(m_unWaitCount==0){
			DoFree()
			return
		}
		}
	# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
	barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
	BroadcastGraph()
  }
}
#
#Do Lock
#
timeout_graph = 40
function DoLock() {
  #[transition to lock...
	BVMSTATE="GRAPH_LOCK"
	m_selfMessage.State=s2i(BVMSTATE)
	m_selfMessage.Label=m_nLabel
	m_vecNodes[m_nLabel].State="ASSIGNED"
	#record neighbor distance
	lock_neighbor_id={}
	lock_neighbor_dis={}
	var i=0
	while(i<m_neighbourCount){
		lock_neighbor_id[i]=m_messageID[i]
		lock_neighbor_dis[i]=m_MessageRange[i]
		i=i+1
	}
	m_navigation.x=0.0
	m_navigation.y=0.0
	goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)

	#stop listening
	neighbors.ignore("m")
  #..]
	UpdateGraph()
	m_selfMessage.State=s2i(BVMSTATE)
	m_selfMessage.Label=m_nLabel
	m_navigation.x=0.0
	m_navigation.y=0.0
	#calculate motion vection
	if(m_nLabel==0){
		m_navigation.x=0.0	#change value so that robot 0 will move
		m_navigation.y=0.0
	}
	if(m_nLabel!=0){
		m_navigation=motion_vector()
	}
	#  #move

	#  goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
	BroadcastGraph()

	if(graph_loop) {
	  if(timeout_graph==0) {
		if(graph_id < 3)
			graph_id = graph_id + 1
		else
			graph_id = 0
		timeout_graph = 40
		BVMSTATE="TASK_ALLOCATE"
	  }
	 	timeout_graph = timeout_graph - 1
	}
}

#
# Executed at init
#
function initGraph() {
	#
	# Global parameters for graph formation
	#
	m_unResponseTimeThreshold=10
	m_unLabelSearchWaitTime=10
	m_fTargetDistanceTolerance=100
	m_fTargetAngleTolerance=0.1
	m_unJoiningLostPeriod=100
}
#
# Executed every step (main loop)
#
function UpdateGraph() {
	#update the graph
	UpdateNodeInfo()
	#reset message package to be sent
	m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
}

function BroadcastGraph() {
	#navigation
	#broadcast message
	neighbors.broadcast("m",packmessage(m_selfMessage))

	#
	#clean message storage
	m_MessageState={}#store received neighbour message
	m_MessageLabel={}#store received neighbour message
	m_MessageReqLabel={}#store received neighbour message
	m_MessageReqID={}#store received neighbour message
	m_MessageResponse={}#store received neighbour message
	m_MessageRange={}#store received neighbour message
	m_MessageBearing={}#store received neighbour message
	m_neighbourCount=0

}
#
# Executed when reset
#
function resetGraph(){
	m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
	m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
	m_navigation={.x=0,.y=0}
	m_nLabel=-1
	m_messageID={}
	lock_neighbor_id={}
	lock_neighbor_dis={}
	m_unRequestId=0
	m_bias=0
	m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
	m_unWaitCount=0
	repeat_assign=0
	assign_label=-1
	assign_id=-1
	m_gotjoinedparent = 0

	if(graph_id==0){
		log("Loading P graph")
		Read_GraphP()
	} else if(graph_id==1) {
		log("Loading O graph")
		Read_GraphO()
	} else if(graph_id==2) {
		log("Loading L graph")
		Read_GraphL()
	} else if(graph_id==3) {
		log("Loading Y graph")
		Read_GraphY()
	}

	#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==ROOT_ID){
		m_nLabel=0
		DoJoined()
	}else{	#[B]Other robots are defined as free.
		DoFree()
	}
}
#
# Executed upon destroy
#
function destroyGraph() {
	#clear neighbour message
	m_navigation.x=0.0
	m_navigation.y=0.0
	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
	m_vecNodes={}
	#stop listening
	neighbors.ignore("m")
}