#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time
import can
import cantools
import rospy
import signal
import os
import threading
import std_msgs

is_exit = False

messages = [
    {
        'message_id': 0x1820FF2F,  #messages1
        'message_name': 'Handshaking',
        'signals': {
            'HandShake_name1': 0x74,
            'HandShake_name2': 0x63,
            'HandShake_name3': 0x64,
            'HandShake_name4': 0x72,
            'HandShake_name5': 0x69,
            'HandShake_name6': 0x30,
            'HandShake_name7': 0x30,
            'HandShake_name8': 0x31
        }
    },
    {
        'message_id': 0x0C20112F,  # messages4
        'message_name': 'ElectronicHandbrake_State',
        'signals': {
            'EleHandbrakeStatus': 0,
            'Ele_activate_Status': 0,
            'Ele_checksum': 0,
            'Ele_count': 1,
            'reserve14': 0,
            'reserve15': 0,
            'reserve16': 0,
            'reserve17': 0,
            'reserve18': 0
        }
    },
    {
        'message_id': 0x0C20302F,  # messages7
        'message_name': 'GearPosition',
        'signals': {
            'GearCommand': 0,
            'GearHighest': 6,
            'gear_activate': 0,
            'gear_checksum': 0,
            'gear_count': 1,
            'reserve19': 0,
            'reserve20': 0,
            'reserve21': 0,
            'reserve22': 0
        }
    },
    {
        'message_id': 0x0C20102F, #messages3
        'message_name': 'Brake_decel',
        'signals': {
            'BrakeDecel': 0,
            'Decel_activate_Status': 0,
            'Decel_checksum': 0,
            'Decel_count': 1,
            'reserve5':225,
            'reserve6':225,
            'reserve7':225,
            'reserve8':225
        }
    },#41 74 FF FF FF FF 00 3F
    {
        'message_id': 0x0C20122F, #messages5
        'message_name': 'RetarderTorque_Percent',
        'signals': {
            'Retarder_per': 0,
            'Retarder_activate': 0,
            'Retarder_checksum': 0,
            'Retarder_count': 1,
            'reserve23':0,
            'reserve24':0,
            'reserve25':0,
            'reserve26':0,
            'reserve27':0

        }
    },
    {
        'message_id': 0x0C20002F,  # messages2
        'message_name': 'AccPedal_percent',
        'signals': {
            'AccPedal_per': 0,
            'Acc_activate_Status': 0,
            'Acc_activate': 0,
            'Acc_checksum': 0,
            'Acc_count': 1,
            'reserve1': 0,
            'reserve2': 0,
            'reserve3': 0,
            'reserve4': 0
        }
    },
    {
        'message_id': 0x0C20212F, #messages6
        'message_name': 'SteerAngle_Control',
        'signals': {
            'Curvature_command': 0,
            'steer_control': 0,
            'steer_activate': 0,
            'steer_checksum': 0,
            'steer_count': 1,
            'steercontrol_model': 1,
            'reserve28': 0

        }
    },
    {
        'message_id': 0x0C20402F, #messages8
        'message_name': 'Bucket_State',
        'signals': {
            'bucket_command': 0,
            'bucket_activate': 0,
            'bucket_checksum': 0,
            'bucket_count': 1,
            'bucketspeed_model':0,
            'reserve11':0,
            'reserve12':0,
            'reserve13':0,
            'reserve9':0
        }
    }
]
#加载dbc文件
db_file = os.path.join(os.path.abspath('.'), 'src/wirecontrol/config/kk_wiredcontrol.dbc')

database = cantools.database.load_file(db_file)

can_interface = 'socketcan' #根据can接口类型进行更改
bus = can.interface.Bus(channel= 'can0', bustype = can_interface, bitrate = 500000)

cnt = 0
hands_flag = False

def signal_handler(signal, frame):
    global is_exit
    is_exit = True

def callback_from_rviz(msg):
    global hands_flag
    print("hand brake!!!!!")
    hands_flag = False

def send_can_messages(messages):
    global cnt
    if cnt >= 15:
        cnt = 1
    else:
        cnt += 1
    # 发送多个CAN消息
    for message in messages:
        # 创建CAN消息对象
        print(message)
        msg = database.get_message_by_name(message['message_name'])
        data = msg.encode(message['signals'])
        data1 = list(data)
        if message['message_id'] != 0x1820FF2F:
            len0 = len(data1) - 1
            i = 0
            sum = 0
            while i < len0:
                sum = sum + data1[i]
                i = i + 1
            sum = sum + cnt
            data1[len0] = (cnt << 4) | ((sum >> 4) + sum) & 0x0f
        else:
            pass
        can_message = can.Message(arbitration_id=msg.frame_id, data=data1, is_extended_id=True)

        # 发送CAN消息
        bus.send(can_message)

def shakehand_Task():
    global hands_flag
    global messages

    cnt1  = 0
    while cnt1  <= 0.5:
            cnt1  += 0.01
            send_can_messages(messages)
            time.sleep(0.01)
    print("shakehand task finish!")

def main():
    global is_exit
    global hands_flag

    signal.signal(signal.SIGINT, signal_handler)
    rospy.init_node('wirecontrol_sh', anonymous=False)
    rospy.Subscriber("/Handshack", std_msgs.msg.String, callback_from_rviz)
    
    while not is_exit:
        if hands_flag == False:
            hands_flag = True
            ShakehandTask_threading = threading.Thread(target = shakehand_Task)
            ShakehandTask_threading.start()
        
        time.sleep(1)
    rospy.spin()
    

if __name__ == '__main__':
    main()