#!/usr/bin/env python3

import rospy
import ruamel.yaml
import numpy as np
from std_msgs.msg import UInt16
from sensor_msgs.msg import LaserScan
from std_srvs.srv import Trigger


class TandemManager():

    def __init__(self):
        yaml = ruamel.yaml.YAML()
        waypoints_path = rospy.get_param("tandem_run_manager/waypoints_file")
        self.tandem_start_list = []
        self.tandem_end_list = []
        self.tandem_id = 0
        with open(waypoints_path) as file:
            waypoints_yaml = yaml.load(file)
        for i, data in enumerate(waypoints_yaml["waypoints"]):
            if ("tandem_start" in data["point"]):
                self.tandem_start_list.append(i+2)
            if ("tandem_end" in data["point"]):
                self.tandem_end_list.append(i+2)
        self.in_tandem_area = False
        self.stop = False
        ##
        self.waypoint_num = 0
        self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback)
        self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.laserscan_callback)
        ##
        self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger)
        self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger)
        ##
        self.front_range = None
        return


    def waypoint_num_callback(self, msg):
        if (msg.data == self.waypoint_num) or (self.stop): return
        if (msg.data == self.tandem_start_list[self.tandem_id]):
            rospy.loginfo("")
            self.in_tandem_area = True
        elif (msg.data == self.tandem_end_list[self.tandem_id]):
            self.in_tandem_area = False
            self.tandem_id += 1
        self.waypoint_num = msg.data
        return
    

    def laserscan_callback(self, msg):
        if self.front_range is None:
            front = round(-msg.angle_min / msg.angle_increment)
            range = int(round(np.deg2rad(15) / msg.angle_increment))
            self.front_range = [front-range, front+range]
        ##
        if not self.in_tandem_area: return
        ave_range = np.mean(msg.ranges[self.front_range[0]:self.front_range[1]])
        if (not self.stop) and (ave_range < 0.6):
            self.stop_nav()
            self.stop = True
        elif (self.stop) and (ave_range >= 0.7):
            self.resume_nav()
            self.stop = False
        return





if __name__ == '__main__':
    rospy.init_node("map_changer")
    tandem_manager = TandemManager()
    rospy.spin()