import rosbag
import os
import sys
import cv2
import numpy as np
#import pypcd
import sensor_msgs.point_cloud2 as pc2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from cv_bridge import CvBridgeError

head = "#timestamp [ns],Gyro_X [rad s^-1],Gyro_Y [rad s^-1],Gyro_Z [rad s^-1],Accel_X [m s^-2],Accel_Y [m s^-2],Accel_Z [m s^-2]"

def check_bag_time(bag_path,date):
        time0 = time1 = time2 = 0
        with rosbag.Bag(bag_path, 'r') as bag:
                i = 0
                for topic, msg, t in bag.read_messages():
                        if i == 0 :
                                time0 = msg.header.stamp.to_sec()
                        if i == 1 :
                                time1 = msg.header.stamp.to_sec()
                        if i == 2 :
                                time2 = msg.header.stamp.to_sec()
                                break
                if (time0 == time1) and (time2 - time1) == 0.1:
                        date.append(time0 - 0.1) 
                else:
                        date.append(time0) 
                      

def save2png(path,date):
        #  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/')
         with rosbag.Bag(path+".bag", 'r') as bag:
                for topic, msg, t in bag.read_messages():
                        if "/image_raw" in topic :
                                if "00" in topic :
                                        try:
                                                cv_img = bridge.imgmsg_to_cv2(msg, 'bgr8')
                                        except CvBridgeError as e:
                                                print e
                                        timestr = "%.6f" % msg.header.stamp.to_sec()
					print(timestr + "\n")
                                        cv2.imwrite(path+'/cam00/'+timestr+".png", cv_img)
                                elif "01" in topic :
                                        try:
                                                cv_img = bridge.imgmsg_to_cv2(msg, 'bgr8')
                                        except CvBridgeError as e:
                                                print e
                                        timestr = "%.6f" % msg.header.stamp.to_sec()
                                        cv2.imwrite(path+'/cam01/'+timestr+".png", cv_img)
                                elif "02" in topic :
                                        try:
                                                cv_img = bridge.imgmsg_to_cv2(msg, 'bgr8')
                                        except CvBridgeError as e:
                                                print e
                                        timestr = "%.6f" % msg.header.stamp.to_sec()
                                        cv2.imwrite(path+'/cam02/'+timestr+".png", cv_img)
				elif "03" in topic :
                                        try:
                                                cv_img = bridge.imgmsg_to_cv2(msg, 'bgr8')
                                        except CvBridgeError as e:
                                                print e
                                        timestr = "%.6f" % msg.header.stamp.to_sec()
                                        cv2.imwrite(path+'/cam03/'+timestr+".png", cv_img)
          
         bag.close()

def getCommonTime(path,date_1,date_2):
    common = []
    if date_2.count(date_1[0]):             # date_2 ahead of date_1
        num = date_1.count(date_2[-1])      # match the time from the last one of date_2 to the current place of date_1
        for i in range(num,len(date_2)):
            common.append(date_2[i])
            i = i + 1
    else:
        num = date_2.count(date_1[-1])
        for i in range(0,num):
            common.append(num,len(date_1))
            i = i + 1
    with open(path+'/times.txt','w') as w:
        for i in range(0,len(common)):
            w.write("%.6f" % (common[i])+'\n')
    w.close()

def main(argv):
        if len(argv) < 1:
                print "please give a bag path and a time txt path"
                sys.exit(1)
        # date_1 = date_2 = []
        date = []
        path = argv[1].split('.bag')[0]
        # path = save2png(argv[1],date_1)
        save2png(path,date)
        # getCommonTime(path,date_1,date_2)



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