import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, GoalResponse, CancelResponse

from my_robot_interfaces.action import JointControl

import threading
import time


class XclimbotMainNode(Node):

    def __init__(self):
        super().__init__('xclimbot_main_node')

        # 模拟关节状态（真实项目中来自底层驱动 / CAN）
        self.current_joint_angle = [0] * 6
        self.current_joint_running = [0] * 6

        # Action Server
        self._action_server = ActionServer(
            self,
            JointControl,
            'joint_control',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback
        )

        self.get_logger().info('JointControl Action Server started')

    # ---------- Goal 校验 ----------
    def goal_callback(self, goal_request):
        self.get_logger().info(
            f'Received goal: {list(goal_request.goal_angle)}, '
            f'vel_limit={goal_request.vel_limit}'
        )

        # 示例：简单校验
        if len(goal_request.goal_angle) != 6:
            self.get_logger().warn('Invalid goal angle length')
            return GoalResponse.REJECT

        return GoalResponse.ACCEPT

    # ---------- Cancel ----------
    def cancel_callback(self, goal_handle):
        self.get_logger().warn('Received cancel request')
        return CancelResponse.ACCEPT

    # ---------- 执行主逻辑 ----------
    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing JointControl goal')

        feedback = JointControl.Feedback()
        result = JointControl.Result()

        # 标记关节开始运行
        self.current_joint_running = [1] * 6

        rate = self.create_rate(5)  # 5 Hz feedback

        for step in range(20):  # 模拟 4 秒执行过程

            # ---- cancel 检查 ----
            if goal_handle.is_cancel_requested:
                self.get_logger().warn('Goal canceled during execution')

                self.current_joint_running = [0] * 6
                result.success = False
                result.message = 'Goal canceled by client'
                goal_handle.canceled()
                return result

            # ---- 模拟关节运动 ----
            for i in range(6):
                self.current_joint_angle[i] += 1000  # 模拟变化

            # ---- 填充并发送 feedback ----
            feedback.current_angle = self.current_joint_angle
            feedback.current_running = self.current_joint_running
            goal_handle.publish_feedback(feedback)

            self.get_logger().debug(
                f'Feedback sent: {feedback.current_angle}'
            )

            rate.sleep()

        # 执行完成
        self.current_joint_running = [0] * 6
        result.success = True
        result.message = 'Joint motion finished successfully'

        goal_handle.succeed()
        self.get_logger().info('JointControl goal succeeded')

        return result


def main(args=None):
    rclpy.init(args=args)

    node = XclimbotMainNode()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

# ===== 第一课时：函数的定义与使用 =====

# 1. 最简单的函数 - 无参数无返回值
print("--- 示例1：打招呼函数 ---")

def say_hello():
    """这是一个简单的打招呼函数"""
    print("你好！")
    print("欢迎学习Python！")

# 调用函数
say_hello()
say_hello()  # 可以多次调用

# 2. 带参数的函数
print("\n--- 示例2：带参数的函数 ---")

def greet(name):
    """向指定的人打招呼"""
    print("你好，", name, "！")
    print("很高兴见到你！")

greet("小明")
greet("小红")
greet("小刚")

# 3. 带多个参数的函数
print("\n--- 示例3：多个参数 ---")

def introduce(name, age, hobby):
    """自我介绍函数"""
    print(f"大家好，我叫{name}")
    print(f"我今年{age}岁")
    print(f"我喜欢{hobby}")
    print("-" * 30)

introduce("小明", 10, "打篮球")
introduce("小红", 9, "画画")

# 4. 带返回值的函数
print("\n--- 示例4：返回值 ---")

def add(a, b):
    """计算两个数的和"""
    result = a + b
    return result

sum1 = add(5, 3)
sum2 = add(10, 20)
print("5 + 3 =", sum1)
print("10 + 20 =", sum2)

# 5. 实用函数：计算矩形面积
print("\n--- 示例5：计算面积 ---")

def rectangle_area(length, width):
    """计算矩形面积"""
    area = length * width
    return area

area1 = rectangle_area(5, 3)
area2 = rectangle_area(10, 8)
print("长5宽3的矩形面积：", area1)
print("长10宽8的矩形面积：", area2)

# 6. 实用函数：判断奇偶
print("\n--- 示例6：判断奇偶数 ---")

def is_even(number):
    """判断一个数是否为偶数"""
    if number % 2 == 0:
        return True
    else:
        return False

print("4是偶数吗？", is_even(4))
print("7是偶数吗？", is_even(7))
print("10是偶数吗？", is_even(10))

# 7. 实用函数：计算平均分
print("\n--- 示例7：计算平均分 ---")

def calculate_average(score1, score2, score3):
    """计算三门课的平均分"""
    total = score1 + score2 + score3
    average = total / 3
    return average

avg = calculate_average(85, 90, 88)
print("平均分是：", avg)

# 8. 综合练习：温度转换
print("\n--- 示例8：温度转换 ---")

def celsius_to_fahrenheit(celsius):
    """摄氏度转华氏度"""
    fahrenheit = celsius * 9/5 + 32
    return fahrenheit

def fahrenheit_to_celsius(fahrenheit):
    """华氏度转摄氏度"""
    celsius = (fahrenheit - 32) * 5/9
    return celsius

temp_c = 25
temp_f = celsius_to_fahrenheit(temp_c)
print(f"{temp_c}°C = {temp_f}°F")

temp_f2 = 77
temp_c2 = fahrenheit_to_celsius(temp_f2)
print(f"{temp_f2}°F = {temp_c2}°C")

# 9. 带默认参数的函数
print("\n--- 示例9：默认参数 ---")

def power(number, exponent=2):
    """计算幂次方，默认是平方"""
    result = number ** exponent
    return result

print("5的平方：", power(5))
print("2的3次方：", power(2, 3))
print("3的4次方：", power(3, 4))

# 10. 实践项目：成绩管理系统
print("\n--- 综合实践：成绩管理 ---")

def get_grade(score):
    """根据分数返回等级"""
    if score >= 90:
        return "优秀"
    elif score >= 80:
        return "良好"
    elif score >= 60:
        return "及格"
    else:
        return "不及格"

def print_report(name, math, chinese, english):
    """打印成绩单"""
    print(f"\n===== {name}的成绩单 =====")
    print(f"数学：{math}分 - {get_grade(math)}")
    print(f"语文：{chinese}分 - {get_grade(chinese)}")
    print(f"英语：{english}分 - {get_grade(english)}")
    
    avg = calculate_average(math, chinese, english)
    print(f"平均分：{avg:.1f}分 - {get_grade(avg)}")
    print("=" * 30)

print_report("小明", 85, 92, 88)
print_report("小红", 95, 87, 91)