#!/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_data_read=1
    count_cicv_location=1
    with rosbag.Bag(input_dir ,'r') as bag:
      latitude=0
      longitude=0
      speed=0
      Strg_Angle_Real_Value=0
      VCU_Accel_Pos_Value=0
      VCU_BrkPel_Pos_Value=0
      for topic,msg,t in bag.read_messages():    #t代表时间
            #print(topic)
            #print('-------------------------------------------------')
            if topic == "/cicv_location": #/pji_gps #/cicv_location
                count_cicv_location+=1
                latitude=msg.latitude
                longitude=msg.longitude
                speed=pow(pow(msg.velocity_x,2)+pow(msg.velocity_y,2),0.5)*3.6
                if count_cicv_location%10==0:
                    message_location=[str(t)[:-6],Strg_Angle_Real_Value,VCU_Accel_Pos_Value,VCU_BrkPel_Pos_Value,speed,latitude,longitude]
                    writer_object_detection.writerow(message_location)
             
            if topic == "/data_read": # 10hz
                Strg_Angle_Real_Value=msg.Strg_Angle_Real_Value
                VCU_Accel_Pos_Value=msg.VCU_Accel_Pos_Value
                VCU_BrkPel_Pos_Value=msg.VCU_BrkPel_Pos_Value
                count_data_read+=1
                if count_data_read%10==0:
                    message_data =[str(t)[:-6],msg.Strg_Angle_Real_Value,msg.VCU_Accel_Pos_Value,msg.VCU_BrkPel_Pos_Value,speed,latitude,longitude]
                 
                    writer_object_detection.writerow(message_data)

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

   input_dir = sys.argv[1]
   #input_dir='/media/dell/56FAD34BFAD32651/data/jinlong_datareturn/test_1116/data_merge_2023-11-21-14-17-19_brake_12.bag'
   bagname=input_dir.split('/')[-1].split('.')[0]
   output_dir = sys.argv[2]
   #output_dir='/media/dell/56FAD34BFAD32651/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)
   '''
   #msg.Strg_Angle_Real_Value,msg.VCU_Accel_Pos_Value,msg.VCU_BrkPel_Pos_Value