import modulecommpython
import numpy as np  
import time
import sys  


import proto.cameraobjectarray_pb2 as cameraobjectarray_pb2
import proto.decitionspeedlimit_pb2 as decitionspeedlimit_pb2
import proto.gpsimu_pb2 as gpsimu_pb2   # 2024.10.14

from PyModuleCommModule import PyModuleComm
from cameradecision import CameraDecision


time_gpsimu = 0
msg_gpsimu = gpsimu_pb2.gpsimu()

time_objectarray = 0
msg_cameraobjectarray = cameraobjectarray_pb2.cameraobjectarray()

gdecision = CameraDecision()


# 2024.10.14
def gpsimu_callback(arr : np,nsize,time):  
    global msg_gpsimu
    global time_gpsimu
 #   print("Python callback function called from C++!. time: ",time)
 #   print("    size: ",nsize)  
    sub_arr = arr[0:nsize]
    databytes = sub_arr.tobytes()
    msg = gpsimu_pb2.gpsimu()
    msg.ParseFromString(databytes)
    msg_gpsimu = msg
    time_gpsimu = time
# 2024.10.14

def cameraobjectarray_callback(arr : np,nsize,time):
    global msg_cameraobjectarray
    global time_objectarray
    sub_arr = arr[0:nsize]
    databytes = sub_arr.tobytes() #sub_arr.tostring()
    msg = cameraobjectarray_pb2.cameraobjectarray()
    msg.ParseFromString(databytes)
    msg_cameraobjectarray = msg
    time_objectarray = time
 #   length = len(msg_cameraobjectarray.obj)
 #   print("obj size: ",length)
    pass
    
def SendDefDecision(md : PyModuleComm):
    pass

def MakeDecision(md : PyModuleComm,  xobjarray: cameraobjectarray_pb2.cameraobjectarray,xgpsimu: gpsimu_pb2.gpsimu):  # 2024.10.14
    global gdecision
    if time_objectarray == 0 :
        print(" no object")
        return
#    print("map1 point lenth: ",length)
    xdecision = gdecision.CalcDecision(xobjarray,xgpsimu)  # 2024.10.14
    serialized_str = xdecision.SerializeToString()  
    serialized_array = np.frombuffer(serialized_str, dtype=np.uint8)  
    length = len(serialized_array)
    md.SendData(serialized_array,length)
    pass

    

def main():  

    # 初始化一个变量  
    count = 0
    count = 3.5/1.5
    count = int(count)
    print("count = ",count)  


    mc = PyModuleComm("hcp2_gpsimu")  # 2024.10.14
    mc.RegisterRecv(gpsimu_callback)  # 2024.10.14

    mobj = PyModuleComm("signarray")
    mobj.RegisterRecv(cameraobjectarray_callback)

    md = PyModuleComm("decitonspeedlimit")
    md.RegiseterSend(100000,1)
    # 使用while循环，只要count小于10，就继续循环  
    while count < 10:
        MakeDecision(md,msg_cameraobjectarray,msg_gpsimu)
        time.sleep(0.05)

  
    print("循环结束！")  
  
# 调用main函数  
if __name__ == "__main__":  
    main()