from colav_protobuf_utils import ProtoType, Stamp
from colav_protobuf_utils.protobuf_generator import gen_mission_request, VesselType, GoalWaypoint
from colav_protobuf_utils.protobuf_generator import (
    gen_controller_feedback,
    CtrlStatus,
    CtrlMode,
)
from colav_protobuf_utils.protobuf_generator import gen_agent_update
from colav_protobuf_utils.protobuf_generator import (
    gen_obstacles_update,
    gen_static_obstacle,
    gen_dynamic_obstacle,
    StaticObstacleTypeEnum,
    DynamicObstacleTypeEnum,
)
from colav_protobuf_utils.protobuf_generator import (
    gen_mission_response,
    MissionResponseTypeEnum,
)
from colav_protobuf_utils.serialization import serialize_protobuf
from colav_protobuf_utils.deserialization import deserialize_protobuf

# def test_mission_request_protogen_creator():
#     return gen_mission_request(
#         tag="MOCK_MISSION",
#         stamp=Stamp(
#             sec = int(231),
#             nanosec= int(231)
#         ),
#         vessel_tag="MOCK_AGENTS",
#         vessel_type=VesselType.HYDROFOIL,
#         vessel_max_acceleration=float(5.0),
#         vessel_max_deceleration=float(-2.0),
#         vessel_max_velocity=float(30),
#         vessel_min_velocity=float(15.0),
#         vessel_max_yaw_rate=float(0.2),
#         vessel_loa=float(2.0),
#         vessel_beam=float(0.5),
#         vessel_safety_radius=float(2.0),
#         cartesian_init_position=(0,0,0),
#         goal_waypoints=[
#             GoalWaypoint(
#                 position=(0, 0, 0),
#                 acceptance_radius=float(2.0)
#             )
#         ]
#     )

# def test_mission_response_protogen_creator():
#     return gen_mission_response(
#         tag="MOCK_MISSION",
#         stamp=Stamp(
#             sec = int(231),
#             nanosec= int(231)
#         ),
#         response_type=MissionResponseTypeEnum.MISSION_STARTING,
#         response_details="Mission accepted",
#     )

# def test_agent_update_protogen_creator():
#     return gen_agent_update(
#         mission_tag="MOCK_MISSION",
#         agent_tag="MOCK_AGENT",
#         cartesian_position=[0, 0, 0],
#         quaternium_orientation=[0, 0, 0, 1],
#         velocity=15,
#         yaw_rate=0.2,
#         acceleration=0.2,
#         stamp=Stamp(
#             sec = int(231),
#             nanosec= int(231)
#         ),
#     )

# def test_dynamic_obstacle_protogen_creator():
#     return [
#         gen_dynamic_obstacle(
#             tag="MOCK_DYNAMIC_OBSTACLE",
#             type=DynamicObstacleTypeEnum.VESSEL,
#             cartesian_position=[10, 3, 2],
#             quaternium_orientation=[0, 0, 0, 1],
#             velocity=15,
#             yaw_rate=0.2,
#             loa=2,
#             beam=0.5,
#             safety_radius=2,
#         )
#     ]

# def test_static_obstacle_protogen_creator():
#     return [
#         gen_static_obstacle(
#             tag="MOCK_STATIC_OBSTACLE",
#             type=StaticObstacleTypeEnum.BUOY,
#             cartesian_position=[10, 3, 2],
#             quaternium_orientation=[0, 0, 0, 1],
#             polyshape_points=[(0, 0, 0), (0, 0, 0), (0, 0, 0)],
#             inflation_radius=2,
#         )
#     ]

# def test_obstacles_update_protogen_creator():
#     dynamic_obstacle_list = test_dynamic_obstacle_protogen_creator()
#     static_obstacle_list = test_static_obstacle_protogen_creator()

#     return gen_obstacles_update(
#         mission_tag="MOCK_MISSION",
#         dynamic_obstacles=dynamic_obstacle_list,
#         static_obstacles=static_obstacle_list,
#         stamp=Stamp(
#             sec = int(321), 
#             nanosec=int(213)
#         ),
#     )

# def test_controller_feedback_protogen_creator():
#     return gen_controller_feedback(
#         mission_tag="MOCK MISSION",
#         agent_tag="mock agent",
#         ctrl_mode=CTRLMode.CRUISE,
#         ctrl_status=CTRLStatus.ACTIVE,
#         velocity=float(15),
#         yaw_rate=float(0.2),
#         stamp=Stamp(
#             sec = int(231),
#             nanosec=int(131)
#         )
#     )



def test_deserialize_mission_request():
    pass

def main():
    pass
    # proto_mission_request = test_mission_request_protogen_creator()
    # serialised_mission_request = serialize_protobuf(proto_mission_request)
    # deserialised_mission_request = deserialize_protobuf(serialised_mission_request, proto_type=ProtoType.MISSION_REQUEST)
    # print (deserialised_mission_request)
    
    # proto_mission_response = test_mission_response_protogen_creator()
    # print (proto_mission_response)
    # serialised_mission_response = serialize_protobuf(proto_mission_response)
    # deserialised_mission_response = deserialize_protobuf(serialised_mission_response, proto_type=ProtoType.MISSION_RESPONSE)
    # print (deserialised_mission_response)
    
    # proto_agent_update = test_agent_update_protogen_creator()
    # serialised_agent_update = serialize_protobuf(proto_agent_update)
    # deserialised_agent_update = deserialize_protobuf(serialised_agent_update, proto_type=ProtoType.AGENT_UPDATE)
    # print (deserialised_agent_update)

    # proto_obstacles_update = test_obstacles_update_protogen_creator()
    # serialised_obstacles_update = serialize_protobuf(proto_obstacles_update)
    # deserialised_obstacles_update = deserialize_protobuf(serialised_obstacles_update, proto_type=ProtoType.OBSTACLES_UPDATE)
    # print (deserialised_obstacles_update)
    
    # proto_controller_feedback = test_controller_feedback_protogen_creator()
    # serialised_controller_feedback = serialize_protobuf(proto_controller_feedback)
    # deserialzed_controller_feedback = deserialize_protobuf(serialised_controller_feedback, proto_type=ProtoType.CONTROLLER_FEEDBACK)
    # print (deserialzed_controller_feedback)

if __name__ == "__main__":
    main()
