########################################
#
# GLOBAL VARIABLES / PARAMETERS
#
########################################

CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv"
OUTPUT_FILENAME_AND_PATH = "/home/ubuntu/bidding_output/output-" # automatically completed with 'ID.csv'
BID_WAIT = 40
PICTURE_WAIT = 40
BASE_ALTITUDE = 5.0
OFFSET_LAT = 0.0 # Switzerland (CSV) to MTL (ROSBuzz)
OFFSET_LON = 0.0 # Switzerland (CSV) to MTL (ROSBuzz)
#
waypoints = {}
#
highest_bid = -2
highest_area = -2
#
bid_made = 0
bidded_area = -1
bid_time = 999999
picture_time = 999999
#
current_area_wp_order = {}
current_area_wp_number = 0
current_area_wp_index = 0

########################################
#
# UTILITY FUNCTIONS
#
########################################

# function name: read_from_csv
# description: read a csv file (with header 'area,type,latitude,longitude,altitude,IMG_XXXX.JPG') containing the list of waypoints
# inputs: a string with the full path and filename of the csv to read
# output: n/a, the function writes in the global variable 'waypoints'
function read_from_csv(s) {
    var csv_file=io.fopen(s, "r")
    csv_entry = 0
    csv_area_counter = 0
    csv_area_id = -1
    csv_wp_counter = 0 
    io.fforeach(csv_file, function(line) {
        csv_wp_counter = csv_wp_counter + 1
        var csv_line_length = string.length(line)
        var csv_value_begin_i = 0
        var csv_scanner_i = 0
        var csv_column = 0
        
        while (csv_scanner_i < csv_line_length) { 
            if (string.sub(line, csv_scanner_i, csv_scanner_i+1) == ',') {
                waypoints[csv_entry] = string.tofloat(string.sub(line, csv_value_begin_i, csv_scanner_i))
                if (csv_column == 0) {
                    if (waypoints[csv_entry] != csv_area_id) {
                        csv_area_id = waypoints[csv_entry]
                        csv_area_counter = csv_area_counter + 1
                    }
                }
                csv_entry = csv_entry + 1
                csv_value_begin_i = csv_scanner_i + 1
                csv_column = csv_column + 1
            } 
            csv_scanner_i = csv_scanner_i + 1
        }
        waypoints[csv_entry] = string.sub(line, csv_value_begin_i, csv_scanner_i)
        csv_entry = csv_entry + 1      
    })
    #table_print(waypoints)
    io.fclose(csv_file)
    NUM_AREAS = csv_area_counter
    NUM_WP = csv_wp_counter
}

# function name: table_print
# description: printout the content of a dictionary
# inputs: the dictionary to print out
# output: n/a, the function print to terminal
function table_print(t) {
    foreach(t, function(key, value) {
        log(key, " -> ", value)
    })
}

# function name: wp_area
# description: obtain the area associated to a waypoint
# inputs: the waypoint number
# output: the area that the waypoint belongs to
function wp_area(wp_number) {
    return waypoints[wp_number * 6 + 0]
}

# function name: wp_type
# description: obtain the type associated to a waypoint
# inputs: the waypoint number
# output: the type of the waypoint
function wp_type(wp_number) {
    return waypoints[wp_number * 6 + 1]
}

# function name: wp_lat
# description: obtain the latitude of a waypoint
# inputs: the waypoint number
# output: the latitude of the waypoint
function wp_lat(wp_number) {
    return waypoints[wp_number * 6 + 2] - OFFSET_LAT # transform to MISTLab's ROSBuzz coordinates
}

# function name: wp_lon
# description: obtain the longitude of a waypoint
# inputs: the waypoint number
# output: the longitude of the waypoint
function wp_lon(wp_number) {
    return waypoints[wp_number * 6 + 3] - OFFSET_LON # transform to MISTLab's ROSBuzz coordinates
}

# function name: wp_alt
# description: obtain the altitude of a waypoint
# inputs: the waypoint number
# output: the altitude of the waypoint
function wp_alt(wp_number) {
    return waypoints[wp_number * 6 + 4]
}

# function name: wp_filename
# description: obtain the filname of a waypoint's image
# inputs: the waypoint number
# output: the filename of the image taken at the waypoint
function wp_filename(wp_number) {
    return waypoints[wp_number * 6 + 5]
}

# function name: distance_from_gps
# description: compute the distance, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
# inputs: the latitude and longitude of the point whose distance we want to compute
# output: the distance, in meters, between the drone and the input (latitude, longitude) pair
function distance_from_gps(lat, lon) {
	var x_lon = lon - pose.position.longitude
  	var x_lat = lat - pose.position.latitude
  	var ned_xx = x_lat/180*math.pi * 6371000.0
  	var ned_yy = x_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi)
	return math.sqrt(ned_xx*ned_xx+ned_yy*ned_yy)	
}

# function name: distance_between_coord
# description: compute the distance, in meters, between the twop (latitude, longitude) pairs of GPS coordinates
# inputs: the latitude and longitude of the first and second point whose distance we want to compute
# output: the distance, in meters, between the two (latitude, longitude) pairs
function distance_between_coord(lat1, lon1, lat2, lon2) {
	var x_lon = lon2 - lon1
  	var x_lat = lat2 - lat1
  	var ned_xx = x_lat/180*math.pi * 6371000.0
  	var ned_yy = x_lon/180*math.pi * 6371000.0 * math.cos(lat2/180*math.pi)
	return math.sqrt(ned_xx*ned_xx+ned_yy*ned_yy)	
}

# function name: x_from_gps
# description: compute the X AXIS SEPARATION, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
# inputs: the latitude and longitude of the point whose distance we want to compute
# output: the X AXIS SEPARATION, in meters, between the drone and the input (latitude, longitude) pair
function x_from_gps(lat, lon) {
  	var x_lat = lat - pose.position.latitude
  	return x_lat/180*math.pi * 6371000.0	
}

# function name: y_from_gps
# description: compute the Y AXIS SEPARATION, in meters, between the drone and a (latitude, longitude) pair of GPS coordinates
# inputs: the latitude and longitude of the point whose distance we want to compute
# output: the Y AXIS SEPARATION, in meters, between the drone and the input (latitude, longitude) pair
function y_from_gps(lat, lon) {
	var x_lon = lon - pose.position.longitude
  	return x_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi)	
}

# function name: sc_move_gps
# description: shortcut function to use the primitive 'uav_moveto' to move the drone towards a (latitude, longitude) pair of GPS coordinates
# inputs: the latitude and longitude of the point that we want to approach
# output: n/a, the drone will move
function sc_move_gps(lat, lon) {
    #var a_coeff = 14.5
    #var x_setpoint = x_from_gps(lat, lon)
    #var y_setpoint = y_from_gps(lat, lon)
    #var d_setpoint = distance_from_gps(lat, lon) 
    
    #if (d_setpoint > 15.0) {
	
    #    x_setpoint = a_coeff * x_setpoint/d_setpoint
    #    y_setpoint = a_coeff * y_setpoint/d_setpoint
    #} else {
    #    ;
    #}

    #log("Distance from", current_area_wp_index, "-th wp in current area", bidded_area," ->", d_setpoint) # USEFUL FOR DEBUGGING
    #goto_abs(x_setpoint, y_setpoint, 0.0, 0.0)
    m_navigation = vec_from_gps(lat,lon, 0)
	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
    if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
    else {
		log("Distance from", current_area_wp_index, "-th wp in current area", bidded_area," ->", math.vec2.length(m_navigation)) # USEFUL FOR DEBUGGING
		m_navigation = LimitSpeed(m_navigation, 1.0)
		#m_navigation = LCA(m_navigation)
		goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
         }       
}

# function name: sc_move_wp
# description: shortcut function to use the primitive 'uav_moveto' to move the drone towards a waypoint
# inputs: the index in the dictionary 'waypoints' of the waypoint that we want to approach
# NOTE: '-1' is a special input that will drive the drone back to its homepoint
# output: n/a, the drone will move
function sc_move_wp(m_wp_i) {
    if (m_wp_i == -1) {
        #log("WARNING: moving to homepoint, if homewpoint was not initialized the script will crash")
        sc_move_gps(HOME_LAT, HOME_LON) # CAREFUL: homepoint initalized at the end of the first take off
    } else {
        sc_move_gps(wp_lat(m_wp_i), wp_lon(m_wp_i))
    }
}

# function name: sc_cover_assigned_area
# description: shortcut function to make a drone reach all the waypoints in an area
# inputs: n/a
# NOTE: the functions exploits the global variables 'current_area_wp_order', 'current_area_wp_number', 'current_area_wp_index'
# NOTE: these MUST be set before its use
# output: n/a, the drone will move
function sc_cover_assigned_area() {
    var pursuing_wp = current_area_wp_order[current_area_wp_index]
    sc_move_wp(pursuing_wp)
    if (distance_from_gps(wp_lat(pursuing_wp), wp_lon(pursuing_wp))<0.1) {
        io.fwrite(output_file, string.concat(   string.tostring(wp_area(pursuing_wp)), ",",
                                                string.tostring(wp_type(pursuing_wp)), ",", 
                                                string.tostring(wp_lat(pursuing_wp)), ",", 
                                                string.tostring(wp_lon(pursuing_wp)), ",", 
                                                string.tostring(wp_alt(pursuing_wp)), ",", 
                                                wp_filename(pursuing_wp), ",", 
                                                string.tostring(id)     ))
        if (current_area_wp_index < (current_area_wp_number - 1)) {
            current_area_wp_index = current_area_wp_index + 1
        } else {
            return 1
        }
    }
    return 0
}

########################################
#
# BIDDING
#
########################################

# function name: drone2area_closest_wp
# description: compute the closest waypoint in an area from the current position of the drone
# inputs: the area id
# output: the id of the closest waypoint in an area
function drone2area_closest_wp(area_id) {
    var wp_i = 0
    var dist = 6371000.0
    var closest_wp = -1
    while (wp_i<NUM_WP) {
        if (wp_area(wp_i) == area_id) {
            var temp = distance_from_gps(wp_lat(wp_i), wp_lon(wp_i))
            if (temp < dist) {
                dist = temp
                closest_wp = wp_i
            }
        }
        wp_i = wp_i + 1
    }
    return closest_wp
}

# function name: drone2area_dist
# description: compute the distance of the closest waypoint in an area from the current position of the drone
# inputs: the area id
# output: the distance of the closest waypoint in an area
function drone2area_dist(area_id) {
    return distance_from_gps(wp_lat(drone2area_closest_wp(area_id)), wp_lon(drone2area_closest_wp(area_id)))
}

# function name: drone2area_path
# description: compute the length of the greedy traversal of all point in an area from the current position of the drone
# inputs: the area id
# output: the lenght of the path over all the waypoints in the area, the function writes into the global variables 'current_area_wp_order', 'current_area_wp_number'
# NOTE: the writing of 'current_area_wp_order', 'current_area_wp_number' is affected by whether 'drone_assigned2area' is set or not
function drone2area_path(area_id) {
    var path = 0.0
    var temp_wp_list = {}
    var wp_i = 0
    var copy_i = 0
    while (wp_i<NUM_WP) {
        if (wp_area(wp_i) == area_id) {
            temp_wp_list[copy_i] = wp_i
            copy_i = copy_i + 1
        }
        wp_i = wp_i + 1
    }
    var wp_in_area = copy_i 
    if (drone_assigned2area == 0) {
        current_area_wp_number = wp_in_area
    }   
    var current_wp = drone2area_closest_wp(area_id)
    var seg_i = 0
    if (drone_assigned2area == 0) {
        current_area_wp_order[seg_i] = current_wp
    }
    while (seg_i<(wp_in_area-1)) {
        var wp_i_2 = 0
        var next_wp_dist = 6371000.0
        var next_wp = -1
        while (wp_i_2 < wp_in_area) {
            if (temp_wp_list[wp_i_2] != -1) {
                if (temp_wp_list[wp_i_2] == current_wp) {
                    temp_wp_list[wp_i_2] = -1
                } else {
                    var temp_seg = distance_between_coord(wp_lat(current_wp), wp_lon(current_wp), wp_lat(temp_wp_list[wp_i_2]), wp_lon(temp_wp_list[wp_i_2]))
                    if (temp_seg < next_wp_dist) {
                        next_wp_dist = temp_seg
                        next_wp = temp_wp_list[wp_i_2]
                    }
                }
            }
            wp_i_2 = wp_i_2 + 1
        }
        #if (next_wp == -1) { log("WARNING: couldn't find next wp") }
        path = path + next_wp_dist
        current_wp = next_wp
        seg_i = seg_i + 1
        if (drone_assigned2area == 0) {
            current_area_wp_order[seg_i] = current_wp
        }
    }
    return path
}

# function name: stig_set_bid
# description: set the bid for an area in 'bidding_stigmergy', the bidder is automatically set to 'id'
# inputs: the area id and the bid to be set
# output: n/a
function stig_set_bid(area_id, m_bid) {
    bidding_stigmergy.put(area_id * 3 + 0, m_bid)
    bidding_stigmergy.put(area_id * 3 + 1, id)
}

# function name: stig_set_status
# description: set the status of an area in 'bidding_stigmergy'
# inputs: the area id and the status to be set
# output: n/a
function stig_set_status(area_id, stat) {
    bidding_stigmergy.put(area_id * 3 + 2, stat)
}

# function name: stig_remove_bid
# description: resets the bid and bidder of an area in 'bidding_stigmergy'
# inputs: the area id
# output: n/a
function stig_remove_bid(area_id) {
    bidding_stigmergy.put(area_id * 3 + 0, -1)
    bidding_stigmergy.put(area_id * 3 + 1, -1)
}

# function name: stig_get_bid
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current bid
# inputs: the area id
# output: the status of area, if initialized, -1 otherwise
function stig_get_bid(area_id) {
    var return_val = bidding_stigmergy.get(area_id * 3 + 0)
    if (return_val != nil) {
        return return_val
    } else {
        return -1
    }
}

# function name: stig_get_bidder
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current bidder
# inputs: the area id
# output: the status of area, if initialized, -1 otherwise
function stig_get_bidder(area_id) {
    var return_val = bidding_stigmergy.get(area_id * 3 + 1)
    if (return_val != nil) {
        return return_val
    } else {
        return -1
    }
}

# function name: stig_get_status
# description: read the local copy of 'bidding_stigmergy' for the value of an area's current status
# inputs: the area id
# output: the status of area, if initialized, 0 otherwise
function stig_get_status(area_id) {
    var return_val = bidding_stigmergy.get(area_id * 3 + 2)
    if (return_val != nil) {
        return return_val
    } else {
        return 0
    }
}

# function name: print_out_bidding_stigmergy
# description: prints all the entries in the local copy of 'bidding_stigmergy'
# inputs: n/a
# output: n/a, the function print to terminal
function print_out_bidding_stigmergy() {
    var k = 0
    while (k < NUM_AREAS) {
        log("Area", k, "(", stig_get_bid(k), ",", stig_get_bidder(k), ",", stig_get_status(k), ")")
        k = k + 1
    }
}

########################################
#
# MAIN FUNCTIONS
#
########################################

# executed once at init time
function init_bidding() {
    
    # read the csv file with the waypoints information
    #read_from_csv(CSV_FILENAME_AND_PATH)
    
    # read the csv file using hook in rosbuzz
    #storegoal(-1.0, -1.0, -1.0)

    # create bidding stigmergy
    bidding_stigmergy = stigmergy.create(1)
    
    # inital take off status
    taken_off = 0
    
    # flag to log the initial latitude and longitude
    logged_homepoint = 0
    
    # flag to make the drone fly back home
    go_home = 0
    
    #flag to state whether a drone is assigned to an area or not
    drone_assigned2area = 0
    
    # initalize iteration counter
    experiment_iteration = 0
    
    #open the output file
    output_file=io.fopen(string.concat(OUTPUT_FILENAME_AND_PATH, string.tostring(id), ".csv"), "w")
    
    log("Drone", id, "initialized")

}

# executed at each time step
function bidding() {

	# enable debug will increase the message size
        debug = 0

    	# read the csv file with the waypoints information
        read_from_csv(CSV_FILENAME_AND_PATH)

	log("experiment_iteration: ", experiment_iteration)

	if (experiment_iteration > 5){
	# save homepoint
	if (logged_homepoint == 0) {
		HOME_LAT = pose.position.latitude
		HOME_LON = pose.position.longitude
		logged_homepoint = 1
	}
	}

        ################################################
        ################# TAKE OFF #####################
        ################################################
        ################################################
    
	# takeoff
	#if (pose.position.altitude < (2 * id + BASE_ALTITUDE) and taken_off == 0){
	#	uav_takeoff(2 * id + BASE_ALTITUDE + 0.1)
	#	log("Drone", id, "is taking-off") # CAREFUL: take off might be unresponsive at times (ROSBuzz fix required?) notes: 'Got command: 22', 'Reply: 1' (apparent success)
	#} else {
	#	taken_off = 1	
	#}
        #}
        #################################################
        ################################################
	
	taken_off = 1

    # in the air, switch between a 'bid and evaluate bids' mode and a 'cover assigned area' mode
    if (taken_off == 1 and experiment_iteration > 10){
        if (drone_assigned2area == 0) {
            # go home is the appropriate flag was set (no unassigned areas left)
            if (go_home) {
                log("Drone", id, "is going home")
                sc_move_wp(-1) # requires a set homepoint
            } else {
                ########################################
                # BIDDING BLOCK START ##################
                ########################################   
                
                ######################################## 
                # EVALUATE BID RESULT ################## 
                ########################################           
                # if bid was won, assign area. otherwiese reset 'bid_made', 'bidded_area'
                if (experiment_iteration>(bid_time+BID_WAIT)) { # long wait between bids evaluations to allow other drones to bid (relatively high comp. time)

                    if (bid_made == 1) {
                        #check if the bid was won
                        var winner = stig_get_bidder(bidded_area)
                        if (winner == id) {
                            log("Drone", id, "decided it has won area", bidded_area, "at iter", experiment_iteration)
                            stig_set_status(bidded_area, 1)
                            drone2area_path(bidded_area) # IMPORTANT: COMPUTE AREA WP ORDER AND NUMBER
                            drone_assigned2area = 1 # IMPORTANT: MAKE THE DRONE SWITCH TO COVERAGE (ALSO BLOCK THE OVER-WRITING OF THE AREA WP ORDER)
                        } else {
                            # free bid flags
                            log("Drone", id, "decided it has lost area", bidded_area, "at iter", experiment_iteration)
                            bid_made = 0
                            bidded_area = -1
                        }
                    }
                }
                
                # if I haven't a currently set bid (NOTE: IT IS IMPORTANT TO COMPUTE BIDS AND TRY TO WRITE STIGMERGY IN DIFFERENT CONTROL STEPS)
                if (bid_made == 0) {
		    picture_time_set = 0
                    ######################################## 
                    # COMPUTE BID ##########################
                    ########################################
                    if (experiment_iteration%2==0) {
                        # find my own highest bid, if any exists 
                        highest_bid = -1
                        highest_area = -1
                        var i = 0
                        while (i<NUM_AREAS) {
                            # only bid on unassigned/uncovered areas
                            if (stig_get_status(i) == 0) {
                                var temp_bid = 1000/(drone2area_dist(i) + drone2area_path(i))
                                if (temp_bid > highest_bid) {
                                    highest_bid = temp_bid
                                    highest_area = i
                                }
                            }
                            i = i + 1
                        }
                        log("Drone", id, "found its highest bid to be", highest_bid, "on", highest_area)
                    }
                    ######################################## 
                    # PLACE BID ############################
                    ########################################
                    if (experiment_iteration%2==1) {
                        if (highest_area == -2) {
                            ;
                        } else {
                            # if no areas are available, set a flag to return home
                            if (highest_area == -1) {
                                go_home = 1
                            } else {
                                # bid (own highest only, if own bid is higher than existing and the wp status is not take)
                                if ( (highest_bid > stig_get_bid(highest_area)) and (stig_get_status(highest_area) == 0) ) {
                                    stig_set_bid(highest_area, highest_bid)
                                    bid_made = 1
                                    bid_time = experiment_iteration
                                    bidded_area = highest_area
                                    log("Drone", id, "SET its bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
                                } else {
                                    bid_made = 0
                                    bidded_area = -1
                                    log("Drone", id, "GAVE UP bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
                                }                    
                            }
                        }    
                    }
                }               
                ########################################
                # BIDDING BLOCK END ####################
                ########################################
            }   
        } else {
            log("Drone", id, "is covering area", bidded_area)
            #######################################################
            # DOUBLE CHECK NO OTHER DRONE IS COVERING THE SAME AREA
            #######################################################
            var leave_area = 0
            var stig_assigned = stig_get_bidder(bidded_area)
            if (stig_assigned != id) {
                log("Drone", id, "figured out that", bidded_area, "is actually assigned to", stig_assigned, "on stigmergy")
                leave_area = 1
            }

            var completed = sc_cover_assigned_area() # CAREFUL: this MUST follow a call to 'drone2area_path(area_id)' when 'drone_assigned2area == 0'
	    if ((completed == 1) and (picture_time_set == 0)){
		picture_time_set = 1
  		picture_time = experiment_iteration
		}

            if ( (completed == 1) or (leave_area == 1) ) {
                # free assignment and bidding flags
		if (experiment_iteration > (picture_time+PICTURE_WAIT)) {
                	drone_assigned2area = 0 # IMPORTANT: MAKE THE DRONE SWITCH TO BIDDING (ALSO RE-ENABLE THE OVER-WRITING OF THE AREA WP ORDER)
                	current_area_wp_index = 0 # IMPORTANT: RESET THE AREA WP INDEX
                	bid_made = 0
                	bidded_area = -1
		}
		else {
			log("Drone is taking pictures")
		}
            }	
	    
        }
    }
    
    ######################################## 
    # TEMP DEBUG BLOCK START ###############
    ########################################   
    if (id == 2 and debug == 1) {
        if (experiment_iteration%20==0){
            print_out_bidding_stigmergy()
        }  
        log("===============")
    } 
    ########################################
    # TEMP DEBUG BLOCK END #################
    ########################################
        
    # increase iteration counter    
    experiment_iteration = experiment_iteration + 1

	# log of the drone position
	#log("P", position.latitude, position.longitude, position.altitude, "TO", taken_off, "v03") # may want to log flight.status too
    
}
 
# executed once when the robot (or the simulator) is reset
#function reset() {
    #log("Drone", id, "was reset")
#}
 
# executed once at the end of experiment
function close_bidding() {
    io.fclose(output_file)
    log("Drone", id, "bids farewell")
}