import os
import sys
import argparse
from string import Template
import numpy as np
import cv2

calib_template = Template('''$fx $fy $cx $cy 0
$rx $ry
crop
600 400
''') 

if __name__ == "__main__":
	parser = argparse.ArgumentParser(description='''This script converts the kitti calibration file to dso calibration file. ''')
	parser.add_argument("--sequence", help="the desired sequence to convert")
	parser.add_argument("--kitti_folder", help="the mono-kitti folder, without '/' in the end", default="/home/ivan/ivan/git/work_drivecast2/SLAM/datasets/mono-kitti/sequences")
	
	args = parser.parse_args()
	dataset_path = args.kitti_folder + "/" + args.sequence + "/"
	kitti_calib_file = dataset_path + "calib.txt"
	print(kitti_calib_file)
	
	with open(kitti_calib_file, 'r') as stream:
		lines = (' '.join([x.strip('\n ') for x in stream.readlines() if x.strip('\n ') ])).split(' ')
		if len(lines) != 52:
			print('Issues loading calibration')
			print(lines)

		P0 = np.array([float(x) for x in lines[1:13]]).reshape(3,4)
		P1 = np.array([float(x) for x in lines[14:26]]).reshape(3,4)
		print('P0\n', P0)
		print('P1\n', P1)

		tx = -P1[0,3]/P1[0,0]

		img = cv2.imread(dataset_path + '/image_0/000000.png')
		rx = img.shape[1]
		ry = img.shape[0]

		values = {'fx': P0[0,0], 'fy': P0[1,1], 'cx': P0[0,2], 'cy': P0[1,2], 'rx': rx, 'ry': ry}

		calib = calib_template.substitute(values)
		print(calib)

		with open(dataset_path + '/camera.txt', 'w') as stream2:
			stream2.write(calib)