# coding: utf-8
# !/usr/bin/env python2

import sys

import os
import rospy
import rosbag
import csv
import cv2
from cv_bridge import CvBridge, CvBridgeError
import argparse
import struct
import subprocess
from subprocess import Popen, PIPE

bridge = CvBridge()


def parsebag(f, output_dir, target_topic):
    rosbag_name = f.split("/")[-1].split(".")[0]
    output_path = os.path.join(output_dir, rosbag_name)
    camera_exist_flag = False

    with rosbag.Bag(f, 'r') as bag:
        count = 0
        start_time = bag.get_start_time()
        end_time = bag.get_end_time()
        duration = end_time - start_time
        print('duration', duration)
        topics = bag.get_type_and_topic_info()[1].keys()
        if target_topic in topics:
            for topic, msg, t in bag.read_messages(topics=[target_topic]):
                camera_exist_flag = True
                img = bridge.imgmsg_to_cv2(msg, 'bgr8')
                img_resized = cv2.resize(img, (1920, 1080))
                # img = bridge.compressed_imgmsg_to_cv2(msg, 'bgr8')
                timestr = msg.header.stamp.to_nsec()
                image_name = str(timestr) + ".jpg"
                if not os.path.exists(output_path + '_' + 'image'):
                    os.makedirs(output_path + '_' + 'image')
                output_path_img = os.path.join(output_path + '_' + 'image', image_name)
                cv2.imwrite(output_path_img, img_resized)
                # print(count)
                count = count + 1

    return count, duration, camera_exist_flag


def parse(input_dir, output_dir):
    # input_dir=sys.argv[1]
    # output_dir = sys.argv[2]
    topic = '/image_raw'  # /camera/color/image_raw

    num_count, duration, flag = parsebag(input_dir, output_dir, topic)

    if flag:
        dirt = os.path.join(output_dir, input_dir.split('/')[-1].split('.')[0] + '_' + 'image')
        bagtime = duration
        hz = str(float(num_count) / bagtime)
        print('bagtime=', bagtime)
        print('num_count=', num_count)

        command = ['ffmpeg', '-f', 'image2', '-r', hz, '-pattern_type', 'glob', '-i'] + [dirt + '/*.jpg'] + ['-tag:v',
                                                                                                             'avc1',
                                                                                                             '-y'] + [
                      dirt + '/camera.mp4']
        p = Popen(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        p.communicate()
        print('done')
        return flag, dirt
    else:
        return flag, ''
