#! /usr/bin/env python3
#-*- coding:utf-8 -*-

from math import sqrt
import rospy
from geometry_msgs.msg import Point
from visualization_msgs.msg import MarkerArray, Marker

class path_maker:
    def __init__(self):
        # make node
        rospy.init_node("narrow_path_maker", anonymous=False)

        #subscriber
        rospy.Subscriber("obstacles_markers", MarkerArray, self.makePath)
        # publisher
        self.pub = rospy.Publisher('target_point', Point, queue_size=10)
        self.vis_pub = rospy.Publisher('target_point_vis', Marker, queue_size=10)

        # lookahead point
        self.left_obs = []
        self.right_obs = []
        self.obstacles = []
        self.nearest_obstacles = []
        self.target_point = Point()
        # visualize용 메세지
        self.target_point_vis = Marker()

        rospy.spin()

    def calc_distance(self, obs_x, obs_y):
        p1 = [0, 0]
        p2 = [obs_x, obs_y]
        dis = sqrt(sum((px - qx) ** 2.0 for px, qx in zip(p1, p2)))
        return dis

    def makeVisual(self, target):
        self.target_point_vis.header.frame_id = "velodyne"
        self.target_point_vis.header.stamp = rospy.Time()
        self.target_point_vis.ns = "name space"
        self.target_point_vis.id = 0
        self.target_point_vis.type = 8
        self.target_point_vis.action = 0
        self.target_point_vis.pose.position.x = 0
        self.target_point_vis.pose.position.y = 0
        self.target_point_vis.pose.position.z = 0
        self.target_point_vis.pose.orientation.x = 0.0
        self.target_point_vis.pose.orientation.y = 0.0
        self.target_point_vis.pose.orientation.z = 0.0
        self.target_point_vis.pose.orientation.w = 1.0
        self.target_point_vis.scale.x = 0.5
        self.target_point_vis.scale.y = 0.5
        self.target_point_vis.scale.z = 0.5
        self.target_point_vis.color.r = 255
        self.target_point_vis.color.g = 0
        self.target_point_vis.color.b = 0
        self.target_point_vis.color.a = 0.7
        self.target_point_vis.lifetime = rospy.Duration(0.1)
        self.target_point_vis.points.append(target) # 이거로 계속 누적되면 이거를 이용해서 loop closure나 경로 점 모두 보기 가능할수도
    
    def left_right_classification(self, points):
        tmp_left_obs = []
        tmp_right_obs = []
        for point in points:
            if point[1] >= 0:
                tmp_left_obs.append(point)
            else:
                tmp_right_obs.append(point)
        return (tmp_left_obs, tmp_right_obs)

    # 하나의 점만 검출될 경우
    # 이전 스티어링 값 유지
    def singleCOM(self, points):
        pass

    def makeCOM(self, points):
        x_sum = 0
        y_sum = 0
        for point in points:
            x_sum = x_sum + point[0]
            y_sum = y_sum + point[1]
        self.target_point.x = x_sum / len(points)
        self.target_point.y = y_sum / len(points)

    def makePath(self, msg):
        # print(type(msg))
        # 각 장애물의 중앙점 따로 저장-> obstacles의 중앙점, 거리 값 같이 계산
        for obs in msg.markers:
            tmp_obs_dis = self.calc_distance(obs.pose.position.x, obs.pose.position.y)
            self.obstacles.append([round(obs.pose.position.x, 2), round(obs.pose.position.y, 2), round(tmp_obs_dis, 2)])
        
        # 점 정렬 & 4개 점 filtering
        self.obstacles.sort(key = lambda x : x[2])
        if len(self.obstacles) > 4:
            self.obstacles = self.obstacles[0:4]    # 가장 거리가 짧은 4개의 점만 추출

        if len(self.obstacles) == 0:
            pass
        elif len(self.obstacles) == 1:
            self.singleCOM(self.obstacles)
        else:
            self.makeCOM(self.obstacles)
        

        self.makeVisual(self.target_point)

        self.vis_pub.publish(self.target_point_vis)
        self.pub.publish(self.target_point)
        self.obstacles.clear()
        self.target_point_vis.points.clear()
        

if __name__ == "__main__":
    print("PATH_MAKER_ON")
    path_maker()