import rosbag
import os
import sys
#import pypcd
import sensor_msgs.point_cloud2 as pc2
from cv_bridge import CvBridge
import glob

def save2bags(path,bagname):
        # check_bag_time(bag_path,date)
        bridge = CvBridge()
        # path = bag_path.split('.')[0]
        if not os.path.exists(path+'/cam00/'):
                os.makedirs(path+'/cam00/')
        if not os.path.exists(path+'/cam01/'):
                os.makedirs(path+'/cam01/')
        if not os.path.exists(path+'/cam02/'):
                os.makedirs(path+'/cam02/')
        if not os.path.exists(path+'/cam03/'):
                os.makedirs(path+'/cam03/')
        bag0 = rosbag.Bag(path+"/cam00/"+bagname+"_00.bag", 'w')
        bag1 = rosbag.Bag(path+"/cam01/"+bagname+"_01.bag", 'w')
        bag2 = rosbag.Bag(path+"/cam02/"+bagname+"_02.bag", 'w')
        bag3 = rosbag.Bag(path+"/cam03/"+bagname+"_03.bag", 'w')

        with rosbag.Bag(path+"/"+bagname, 'r') as bag:
                for topic, msg, t in bag.read_messages():
                        if "/image_raw" in topic :
                                if "00" in topic :
                                        bag0.write(topic, msg, t )
                               
                                elif "01" in topic :
                                        bag1.write(topic, msg, t )

                                elif "02" in topic :
                                        bag2.write(topic, msg, t )
                                
                                elif "03" in topic :
                                        bag3.write(topic, msg, t )

          
        bag.close()
        bag0.close()
        bag1.close()
        bag2.close()
        bag3.close()


def main(argv):
        if len(argv) < 1:
                print("please give a bag path and a time txt path")
                sys.exit(1)
        path = argv[1]
        baglist = glob.glob(os.path.join(path,"*.bag"))
        for each_bag in baglist:
                print(each_bag)
                save2bags(path,each_bag.split("/")[-1])
                

#function begin
if __name__ =="__main__":
       main(sys.argv)