#!/usr/bin/env python3

# Copyright (C) 2024 Michael Ferguson
# Copyright (C) 2015 Fetch Robotics Inc.
# Copyright (C) 2013-2014 Unbounded Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# Author: Michael Ferguson

import threading
import yaml

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import JointState
from robot_calibration_msgs.msg import CaptureConfig


class CapturePoses(Node):
    last_state_ = None # last joint states

    def __init__(self):
        super().__init__('capture_calibration_poses')
        self.last_state_ = CaptureConfig()
        self.poses = list()

        self.state_sub = self.create_subscription(JointState, "joint_states", self.stateCb, 1)

    def collect(self):
        # Collect samples
        count = 0
        while rclpy.ok():
            print('Move robot to a new sample position:  (type "exit" to quit and save data)')
            resp = input('press <enter>')
            if resp.upper() == 'EXIT':
                print('Exiting...')
                return
            else:
                if self.last_state_ == None:
                    print('Cannot save state')
                    continue
                # Save a pose
                pose = {'features': list(self.last_state_.features),
                        'joints': list(self.last_state_.joint_states.name),
                        'positions': list(self.last_state_.joint_states.position)}
                self.poses.append(pose)
                print("Saving pose %d" % count)
                count += 1

    def save(self):
        # Write data to YAML
        with open('calibration_poses.yaml', 'w') as f:
            yaml.dump(self.poses, f)

    def stateCb(self, msg):
        """ Callback for joint_states messages """
        for joint, position in zip(msg.name, msg.position):
            try:
                idx = self.last_state_.joint_states.name.index(joint)
                self.last_state_.joint_states.position[idx] = position
            except ValueError:
                self.last_state_.joint_states.name.append(joint)
                self.last_state_.joint_states.position.append(position)

if __name__=='__main__':
    rclpy.init()
    try:
        node = CapturePoses()
        thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
        thread.start()
        node.collect()
    finally:
        node.save()
        node.destroy_node()
        rclpy.shutdown()
