#!/home/arm/Desktop/ros2_ws_arm/arm/env/bin/python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
import sys
import time

from smbus2 import SMBus
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo

print("Python:", sys.executable)

class TestServo(Node):
    def __init__(self):
        super().__init__('test_servo')
        self.subscription = self.create_subscription(
            Int32MultiArray,
            'servo_angle',
            self.listener_callback,
            10)
        self.get_logger().info("test_servo node started")

        # Set up I2C bus and PCA9685
        self.bus = SMBus(1)  # I2C-1 is the default on Raspberry Pi
        self.pca = PCA9685(i2c=self.bus)
        self.pca.frequency = 50

        # Create 16 servo channels
        self.servos = [servo.Servo(self.pca.channels[i]) for i in range(16)]

    def listener_callback(self, msg):
        try:
            channel = msg.data[0]
            angle = msg.data[1]
            if 0 <= channel < 16 and 0 <= angle <= 180:
                self.servos[channel].angle = angle
                self.get_logger().info(f"Moved servo {channel} to {angle}°")
            else:
                self.get_logger().warn("Invalid channel or angle")
        except Exception as e:
            self.get_logger().error(f"Error: {e}")

    def destroy_node(self):
        self.pca.deinit()
        self.bus.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = TestServo()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
