#!/usr/bin/env python
# coding: utf-8
import json
# import os

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter

from interfaces.msg import PoseArrayClassAndID


class GetCameraPose(Node):
    def __init__(self, name):
        super(GetCameraPose, self).__init__(name)
        self.sync_subscriber = None
        self.sub_camera_pose = None
        self.sub_hand_pose = None
        self.camera = []
        self.calibration_matrix = None
        self.save = False
        self.collect = False
        self.done = False
        self.done_collecting = False
        # self.filename = "camera_pose_data.json"

        self.declare_parameter('class_name', 'crossboard')
        self.declare_parameter('start_collect_once', False)
        self.declare_parameter('save', False)

        # if not os.path.exists(self.filename):
        #     with open(self.filename, "w", encoding="utf-8") as f:
        #         json.dump([], f, ensure_ascii=False, indent=2)

        self.sub_camera_pose = self.create_subscription(PoseArrayClassAndID, '/pose/cv_detect_pose', self.get_pose_callback, 10)
        self.timer_ = self.create_timer(1, self.timer_callback)

    def get_pose_callback(self, camera_pose):
        if self.collect and not self.done_collecting:
            self.done_collecting = True


            _camera = None

            pose_dict = {}
            for object in camera_pose.objects:
                pose_dict[object.class_name] = object.pose_list

            if self.class_name in pose_dict:
                _camera = [
                    pose_dict[self.class_name][-1].position.x,
                    pose_dict[self.class_name][-1].position.y,
                    pose_dict[self.class_name][-1].position.z,
                    pose_dict[self.class_name][-1].orientation.w,
                    pose_dict[self.class_name][-1].orientation.x,
                    pose_dict[self.class_name][-1].orientation.y,
                    pose_dict[self.class_name][-1].orientation.z,
                ]

                self.get_logger().info(f"add camera: {_camera}")
                self.done_collecting = False
                self.collect = False
                self.camera.extend(_camera)
                self.set_parameters([Parameter('start_collect_once', Parameter.Type.BOOL, False)])

            else:
                self.done_collecting = False
                self.collect = False
                self.set_parameters([Parameter('start_collect_once', Parameter.Type.BOOL, False)])
                self.get_logger().info(f"Have not camera data")
                return

        if self.save:
            print(self.camera)
            self.get_logger().info(f'Save as camera_pose_data.json')
            with open(f"camera_pose_data.json", "w") as f:
                json.dump(self.camera, f, indent=4)

            self.done = True

    def timer_callback(self):
        self.collect = self.get_parameter('start_collect_once').value
        self.save = self.get_parameter('save').value
        self.class_name = self.get_parameter('class_name').value


def main(args=None):
    rclpy.init(args=args)
    node = GetCameraPose('get_camera_pose')
    try:
        while rclpy.ok() and not node.done:
            rclpy.spin_once(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

