#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Dec 25 17:35:56 2023

@author: dell
"""

# coding: utf-8
# !/usr/bin/env python2
import os
import rosbag
import csv
import math

import sys
import time


def parsebag(input_dir, output_dir):
    dic_object_detection = ['Time', 'Strg_Angle', 'Accel_Pos', 'BrkPel_Pos', 'Real_Speed', 'latitude', 'longitude']
    object_detection_file = open(output_dir + "/" + "drive.csv", 'w')
    writer_object_detection = csv.writer(object_detection_file)
    writer_object_detection.writerow(dic_object_detection)

    frame_max = sys.maxsize
    count = 1
    with rosbag.Bag(input_dir, 'r') as bag:
        flag = False

        for topic, msg, t in bag.read_messages():  # t代表时间
            if topic == "/cicv_location":
                flag = True
                latitude = msg.latitude
                longitude = msg.longitude

            if topic == "/data_read":  # 10hz
                count += 1
                if count % 10 == 0 and flag:
                    message_location = [str(t)[:-6], msg.Strg_Angle_Real_Value, msg.VCU_Accel_Pos_Value,
                                        msg.VCU_BrkPel_Pos_Value,
                                        msg.VCU_Real_Speed * 3.6, latitude, longitude]

                    writer_object_detection.writerow(message_location)

        object_detection_file.close()


if __name__ == "__main__":
    #    print("Input the path of file...."+'\n')

    # input_dir = sys.argv[1]
    input_dir = '/media/dell/BFAC-F22B/data/jinlong_datareturn/test_1116/jinlong_data_merge_2023-12-13-05-59-34_TTC_7.bag'
    bagname = input_dir.split('/')[-1].split('.')[0]
    # output_dir = sys.argv[2]
    output_dir = '/media/dell/BFAC-F22B/data/jinlong_datareturn/test_1116/'
    output_dir = os.path.join(output_dir, bagname)
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)
    parsebag(input_dir, output_dir)
    '''
    try:
        parsebag(input_dir, output_dir)
        print('successfully analysis '+input_dir)
    except Exception as e:
        print(e)
    '''


def parse(input_dir, output_dir):
    bagname = input_dir.split('/')[-1].split('.')[0]
    output_dir = os.path.join(output_dir, bagname)
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)
    parsebag(input_dir, output_dir)
    return output_dir