import pandas as pd

def find_intervals_exceeding_threshold(df_ego, threshold=0.2):
    start_time_list = []
    time_length_list = []
    time_list = df_ego[(df_ego["obstacle"] == 1) & (abs(df_ego["speed_linear"]) < 0.01)]["simTime"].tolist()
    # 确保时间列表至少有两个元素
    if len(time_list) < 2:
        return []  # 或者可以抛出一个异常，因为没有足够的数据来比较

    # 初始化变量
    start_time = time_list[0]  # 当前连续时间段的起始时间
    intervals = []  # 存储超过阈值的时间段信息

    # 遍历时间列表（从第二个元素开始）
    for i in range(1, len(time_list)):
        # 计算当前元素与前一个元素的差值
        difference = time_list[i] - time_list[i - 1]

        # 如果差值超过阈值，则记录前一个时间段的信息
        if difference > threshold:
            # 注意：这里我们返回的是前一个时间段的起始和终止时间（以及它们的差值）
            # 但按照题目要求，我们只返回差值
            # 终止时间是前一个元素，即 time_list[i-1]
            # 起始时间是之前记录的 start_time
            interval_length = time_list[i - 1] - start_time
            intervals.append((start_time, time_list[i - 1], interval_length))
            # 更新起始时间为当前元素，以便开始记录下一个时间段
            start_time = time_list[i]

            # 处理最后一个时间段（如果它存在且没有因为超过阈值而被记录）
    if time_list[-1] != start_time:
        # 注意：这里我们假设列表的最后一个元素之后没有更多的元素来比较
        # 因此，我们总是将最后一个元素视为前一个时间段的终止时间
        interval_length = time_list[-1] - start_time
        intervals.append((start_time, time_list[-1], interval_length))
    interval_count = len(intervals)
    for start, end, length in intervals:
        start_time_list.append(start)
        time_length_list.append(length)
        # 如果只需要差值，可以只返回差值的列表
    # return [interval[2] for interval in intervals]

    # 返回包含起始时间、终止时间和差值的元组列表
    return start_time_list, time_length_list, interval_count

if __name__ == "__main__":
    df_ego = pd.read_csv(r"D:\Cicv\朴津项目\pji_outdoor_robot_evaluate_real_0818\task\test_0807-1\EgoState_pji.csv")
    intervals = find_intervals_exceeding_threshold(df_ego)
    print("intervals is", intervals)
    for start, end, length in intervals:
        print(f"时间段从 {start:.2f}s 到 {end:.2f}s，长度为 {length:.2f}s")