#!/usr/bin/python3

import rospy
import json

from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker, MarkerArray


class MapPublisher:

    def __init__(self):
        self.file = rospy.get_param('~file')

        rospy.loginfo('Load map from %s', self.file)
        with open(self.file, 'r') as f:
            data = json.load(f)

        if 'ground_truth' in data:
            color = 0, 1, 0
            objects = data['ground_truth']['objects']
        elif 'results' in data:
            color = 1, 0, 0
            objects = data['results']['objects']

        print('Found', len(objects), 'objects')

        marker_o = Marker()
        marker_o.header.frame_id = 'map'
        marker_o.header.stamp = rospy.Time.now()
        marker_o.type = Marker.LINE_LIST
        marker_o.action = Marker.ADD
        marker_o.scale.x = 0.01
        marker_o.scale.y = 0.01
        marker_o.scale.z = 0.01
        marker_o.color.a = 1
        marker_o.color.r = color[0]
        marker_o.color.g = color[1]
        marker_o.color.b = color[2]
        marker_o.lifetime = rospy.Duration(0)
        marker_o.pose.orientation.w = 1.0

        for obj in objects:
            cx, cy, cz = obj['centroid']
            sx, sy, sz = obj['extent']

            # bottom points
            bA = Point(cx - sx / 2, cy - sy / 2, cz - sz / 2)
            bB = Point(cx - sx / 2, cy + sy / 2, cz - sz / 2)
            bC = Point(cx + sx / 2, cy + sy / 2, cz - sz / 2)
            bD = Point(cx + sx / 2, cy - sy / 2, cz - sz / 2)

            # top points
            tA = Point(cx - sx / 2, cy - sy / 2, cz + sz / 2)
            tB = Point(cx - sx / 2, cy + sy / 2, cz + sz / 2)
            tC = Point(cx + sx / 2, cy + sy / 2, cz + sz / 2)
            tD = Point(cx + sx / 2, cy - sy / 2, cz + sz / 2)

            # bottom rectangle

            marker_o.points.append(bA)
            marker_o.points.append(bB)

            marker_o.points.append(bB)
            marker_o.points.append(bC)

            marker_o.points.append(bC)
            marker_o.points.append(bD)

            marker_o.points.append(bD)
            marker_o.points.append(bA)

            # top rectangle

            marker_o.points.append(tA)
            marker_o.points.append(tB)

            marker_o.points.append(tB)
            marker_o.points.append(tC)

            marker_o.points.append(tC)
            marker_o.points.append(tD)

            marker_o.points.append(tD)
            marker_o.points.append(tA)

            # vertical edges

            marker_o.points.append(bA)
            marker_o.points.append(tA)

            marker_o.points.append(bB)
            marker_o.points.append(tB)

            marker_o.points.append(bC)
            marker_o.points.append(tC)

            marker_o.points.append(bD)
            marker_o.points.append(tD)

        self.pub = rospy.Publisher('/object_map', Marker, queue_size=10)

        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.pub.publish(marker_o)
            r.sleep()

    def run(self):
        rospy.spin()


def main():
    rospy.init_node('map_publisher')
    node = MapPublisher()
    node.run()


if __name__ == '__main__':
    main()