constant_communication.bzz

#Author: Taylor Bergeron


#WORKING BUT NOT FOR 4->6
v = stigmergy.create(1)
s = swarm.create(1)
#in_backbone = 0.0
var min_robot_id_final = -1.0

function init() {
	#v = stigmergy.create(1)
	s.join()
    x1 = 0.0
	y1 = 0.0
	yaw1 = 0.0
    var info = { .x=x1, .y=y1, .yaw=yaw1}
    v.put(id, info)
	id_for_backbone = id+100
	v.put(id_for_backbone, 0.0)
    timestep = 0
}

function step() {

	id_for_backbone = id+100
    i = v.get(id)
	in_backbone = v.get(id_for_backbone)
	#log(in_backbone)
    # in_backbone = i.in_backbone

    var gps = determine_gps()
    if(gps==0.0){
        #this is where shape eploration and estimation would go
        set_wheels(0.0,0.0)
        #neighbors.broadcast("need_backbone", 1.0)

        #var last_info = v.get(id)
        #log(last_info)
        var last_pos_x = i.x
        var last_pos_y = i.y
        var last_yaw = i.yaw
        #var last_backbone = last_info.in_backbone
        #var no_gps_info = { .x=last_pos_x, .y=last_pos_y, .yaw=last_yaw, .in_backbone = -1.0}
        #log(last_pos_x, last_pos_y)

        if(in_backbone == 0.0){
            closest_id = get_closest_id(last_pos_x, last_pos_y)
    		log(id, " chose ", closest_id)
            if(closest_id != -1){
                log(id, " is adding ", closest_id, " to backbone")
                # var other_robots_info = v.get(closest_id)
                # var other_pos_x = other_robots_info.x
                # var other_pos_y = other_robots_info.y
                # var other_yaw = other_robots_info.yaw
                # var new_other_robot_info = { .x=other_pos_x, .y=other_pos_y, .yaw=other_yaw}
				cloest_id_for_backbone = closest_id + 100
                v.put(cloest_id_for_backbone, 1.0)
                if(closest_id==6){
                    var in_six = v.get(106)
                    log("6 got from v: ", sixb)
                }
                v.put(id_for_backbone, 1.0)
            }
        }
        else{
            #var no_gps_info = { .x=last_pos_x, .y=last_pos_y, .yaw=last_yaw, .in_backbone = 0.0}
            #v.put(id, no_gps_info)
        }


    }
    if(gps==1.0){
        x1 = pose["position"]["x"]
    	y1 = pose["position"]["y"]
    	yaw1 = pose["orientation"]["yaw"]

        # neighbors.broadcast("free", 1.0)

		# obstacle_avoidance()

        var infoo = { .x=x1, .y=y1, .yaw=yaw1}
        v.put(id, infoo)

        if(in_backbone == 0.0){
            #chilling, walking around
            neighbors.broadcast("free", 1.0)
            obstacle_avoidance()
        }
        # else if (in_backbone == 0.5){
        #     #going to be a part of the backbone
        #     neighbors.broadcast("going to backbone")
        #     following = i.following
        #     var following_robot_info = v.get(following)
        #     var following_pos_x = following_robot_info.x
        #     var following_pos_y = following_robot_info.y
        #     var following_yaw = following_robot_info.yaw
        #
        # }
        else{
            #successfully part of the backbone DONT MOVE
            # right now, just have robot stop
            log(id, " is in backbone")
            neighbors.broadcast("in_backbone", 1.0)
            set_wheels(0.0,0.0)
        }
    }

}

# function connect_backbone(){
#
# }

function get_closest_id(last_x, last_y){

    min_dist = 100000.0
    #var min_robot_id_final = -1.0

    neighbors.listen("free",
        function(vid, value, rid) {
            #log("id: ", rid, " pos x: ", value)
            # var neighbor_info = v.get(rid)
            # neighbor_in_backbone = neighbor_info.in_backbone
            # if(neighbor_in_backbone==0.0){
            #     difference_x = last_x - neighbor_info.x
            #     difference_y = last_y - neighbor_info.y
            # }
            neighbors.foreach(
                function(robotid, data) {
                if(rid==robotid){
                    #log(id," to ", robotid, " is ", data.distance)
                    if (data.distance < min_dist){
                        #log(id," to ", robotid, " is min from ", min_dist)
                        min_dist = data.distance
                        min_robot_id = robotid
                    }
                } })
            #log("The min robot is: ", min_robot_id)
            min_robot_id_final = min_robot_id
        })
		#log(min_robot_id_final)
		return min_robot_id_final
}

function determine_gps(){
	var bottom_val = ground[0]["value"]
	var gps= 1.0
    if (bottom_val < 1.0){
        gps = 0.0
    }
    return gps

}

function obstacle_avoidance(){
	var magnitude = 5.0
	var obstacle = read_proximity()
	x = magnitude * math.cos(math.pi + obstacle)
	y = magnitude * math.sin(math.pi + obstacle)

	#if angle behind robot, ignore
	if (obstacle == -math.pi){
		#nothing is detected
		set_wheels(magnitude, magnitude)
	}
	else {
		#something is detected
		#isolate cases near 0 to avoid errors
		if (x<0.000001 and x>-0.000001){
			x=0
		}
		if (y<0.000001 and y>-0.000001){
			y=0
		}

		#sets center of the mass of the robot to half x and half y
		#log(id, ": speedx: ",x," speed y: ", y)
		gotoc(x/2, y/2)

	}
}

function read_proximity(){
	var counter = 0.0
	var max_reading = 0.0
	var obstacle_angle = -math.pi

	while (counter < 8.0){
		var sensor_val = proximity[counter].value
		var sensor_angle = proximity[counter].angle
		if(sensor_val > max_reading){
			max_reading = sensor_val
			obstacle_angle = sensor_angle
			#log("Obstacle infront of robot ", id, " at sensor ", counter)
		}
		counter = counter + 1.0
	}
	return obstacle_angle

}

function destroy(){}

function reset(){}