import numpy as np
import pandas as pd

def obstacle_collision(df_ego):
    obstacle_time_list = df_ego[(df_ego["obstacle"] == 1) & (abs(df_ego["speed_linear"]) < 0.01)]["simTime"].tolist()
    print("obstacle_time_list is", obstacle_time_list)
    start_obstacle_time_list = []
    interval = []
    if len(obstacle_time_list) > 0:
        start = obstacle_time_list[0]
        end = obstacle_time_list[0]
        start_obstacle_time_list.append(obstacle_time_list[0])
    for i in range(len(obstacle_time_list) - 1):
        if obstacle_time_list[i+1] - obstacle_time_list[i] > 0.2:
            end = obstacle_time_list[i]
            start_obstacle_time_list.append(obstacle_time_list[i+1])
            interval.append(float("{:.02f}".format(end-start)))
            start = obstacle_time_list[i+1]
            if start == obstacle_time_list[-1]:
                interval.append(float("{:.02f}".format(0)))
        else:
            end = obstacle_time_list[i]
        # else:
        #     interval.append(float("{:.02f}".format(end - start)))
    obstacle_collision_count = len(start_obstacle_time_list)
    return start_obstacle_time_list, obstacle_collision_count, interval

if __name__ == "__main__":
    df_ego = pd.read_csv(r"D:\Cicv\朴津项目\pji_outdoor_robot_evaluate_real_0818\task\test_0807-1\EgoState_pji.csv")
    start_obstacle_time_list, obstacle_collision_count, interval = obstacle_collision(df_ego)
    print("collision is", start_obstacle_time_list, obstacle_collision_count, interval)