#!/usr/bin/env python3
import rospy
import serial
import binascii
import time
from std_msgs.msg import String
from sensor_msgs.msg import BatteryState
from rc_receiver.msg import rc
from geometry_msgs.msg import Twist

class WirelessChargingNode:
    def __init__(self):
        rospy.init_node('wireless_charging_yichuang', anonymous=False)
        
        # 获取参数
        self.port = rospy.get_param('~port', '/dev/ttyS1')
        self.baudrate = rospy.get_param('~baudrate', 115200)
        self.polling_rate = rospy.get_param('~polling_rate', 10)
        self.timeout = rospy.get_param('~timeout', 0.5)
        
        # 初始化串口
        self.ser = None
        self.init_serial()
        
        # 初始化订阅者
        self.attach_result_sub = rospy.Subscriber(
            '/attach/result', 
            String, 
            self.attach_result_callback
        )
        self.battery_state_sub = rospy.Subscriber(
            '/sensor/battery_state', 
            BatteryState, 
            self.battery_state_callback
        )
        self.rc_contrl_sub = rospy.Subscriber(
            '/remote_ctrl', 
            rc, 
            self.rc_ctrl_callback
        )
        self.auto_contrl_sub = rospy.Subscriber(
            '/cmd_vel', 
            Twist, 
            self.auto_ctrl_callback
        )
        
        # 充电状态标志（仅用于日志显示）
        self.is_charging = False
        self.charging_status = False
        self.full_charge_count = 0
        # 用于记录上次处理的消息ID，确保每次新消息都能被处理
        self.last_attach_msg_id = None
        self.last_battery_msg_id = None

        self.car_ctrl_mode = 0
        

        self.rate = rospy.Rate(self.polling_rate)
        rospy.loginfo("Wireless charging node initialized")

    def init_serial(self):
        """初始化串口连接"""
        try:
            self.ser = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                timeout=self.timeout,
                parity='N',
                stopbits=1,
                bytesize=8
            )
            if self.ser.is_open:
                rospy.loginfo(f"Serial port {self.port} opened successfully")
        except serial.SerialException as e:
            rospy.logerr(f"Failed to open serial port: {e}")
            self.ser = None

    def send_start_charging(self):
        """发送开始充电指令"""
        if not self.ser or not self.ser.is_open:
            rospy.logwarn("Serial port not open, cannot send start charging command")
            self.init_serial()  # 尝试重新初始化串口
            return
            
        try:
            # 开始充电指令
            start_cmd = b'\x04\x06\x30\x01\x00\xB1\x17\x2B'   
            self.ser.write(start_cmd)
            rospy.loginfo("Sent start charging command")
            self.is_charging = True
        except Exception as e:
            rospy.logerr(f"Failed to send start charging command: {e}")

    def send_stop_charging(self):
        """发送停止充电指令"""
        if not self.ser or not self.ser.is_open:
            rospy.logwarn("Serial port not open, cannot send stop charging command")
            self.init_serial()  # 尝试重新初始化串口
            return
            
        try:
            # 停止充电指令
            stop_cmd = b'\x04\x06\x30\x01\x00\xB2\x57\x2A'  
            self.ser.write(stop_cmd)
            rospy.loginfo("Sent stop charging command")
            self.is_charging = False
        except Exception as e:
            rospy.logerr(f"Failed to send stop charging command: {e}")

    def attach_result_callback(self, msg):
        """处理/attach/result话题回调 - 每次收到"Finish"都触发"""
        rospy.logdebug(f"Received attach result: {msg.data}")
        
        # 生成消息唯一标识（使用时间戳和消息内容的组合）
        current_msg_id = f"{rospy.get_time()}-{msg.data}"
        
        # 检查是否为新消息且内容为"Finish"
        if msg.data == "Finish" and current_msg_id != self.last_attach_msg_id:
            rospy.loginfo("Received 'Finish' signal, sending start charging command")
            self.send_start_charging()
            time.sleep(0.1)
            self.send_start_charging()
            time.sleep(0.1)
            self.send_start_charging()
            # 更新最后处理的消息ID
            self.last_attach_msg_id = current_msg_id


    def rc_ctrl_callback(self, msg):
        """/remote_ctrl接收到yundong指令发送停充指令"""
        self.car_ctrl_mode = msg.control_mode
        if (msg.linear_x <= -0.01 or msg.linear_x >= 0.01) and self.charging_status == True and self.car_ctrl_mode == 1:
                rospy.loginfo("Received 'back' command, sending stop charging command")
                self.send_stop_charging()

    def auto_ctrl_callback(self, msg):
        """/remote_ctrl接收到yundong指令发送停充指令"""
        if (msg.linear.x <= -0.01 or msg.linear.x >= 0.01) and self.charging_status == True and self.car_ctrl_mode == 0:
                rospy.loginfo("Received 'back' command, sending stop charging command")
                self.send_stop_charging()


    def battery_state_callback(self, msg):
        """处理/sensor/battery_state话题回调 - 满电一段时间发送停充指令"""
        rospy.logdebug(f"Battery percentage: {msg.percentage}")
        
        # 生成消息唯一标识
        current_msg_id = f"{rospy.get_time()}-{msg.percentage}"
        
        # 检查是否为新消息且电量达到100%
        if msg.percentage == 100.0 and current_msg_id != self.last_battery_msg_id:
            self.full_charge_count += 1
            if self.full_charge_count >= 25:
            	rospy.loginfo(f"Battery fully charged ({msg.percentage}%), sending stop charging command")
            	self.send_stop_charging()
            	self.full_charge_count = 0
            # 更新最后处理的消息ID
            self.last_battery_msg_id = current_msg_id
        if msg.percentage < 100.0:
            self.full_charge_count = 0
        if msg.power_supply_status == 1:
            self.charging_status = True
        else:
            self.charging_status = False

    def run(self):
        while not rospy.is_shutdown():
            try:
                if self.ser and self.ser.is_open:
                    self.ser.flushInput()
            except Exception as e:
                rospy.logerr(f"Error in main loop: {e}")
            
            self.rate.sleep()

if __name__ == '__main__':
    try:
        node = WirelessChargingNode()
        node.run()
    except rospy.ROSInterruptException:
        pass
    finally:
        try:
            if 'node' in locals() and hasattr(node, 'ser') and node.ser and node.ser.is_open:
                node.ser.close()
                rospy.loginfo("WirelessCharging serial port closed")
        except Exception as e:
            rospy.logerr(f"Error closing serial port: {e}")