#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Jan  3 12:20:20 2019

@author: matthieu
"""

import numpy as np
import os
import glob
from detect_frustrums import get_rotation_around_axis


# transforms generated by the SLAM
transforms_file = "/home/matthieu/Dev/VeloView-kwinternal/transforms_47.csv"

# load transforms (3x4)
with open(transforms_file, "r") as fin:
    lines = fin.readlines()
transforms = [np.eye(4)[:3, :]]
for i in range(1, len(lines)):      # skip the first line
    if len(lines[i]) > 0:
        temp = list(map(float, lines[i].split(",")))
        t = temp[0]
        roll = temp[1]
        pitch = temp[2]
        yaw = temp[3]
        x = temp[-3]
        y = temp[-2]
        z = temp[-1]
    rx = get_rotation_around_axis(roll, "x")
    ry = get_rotation_around_axis(pitch, "y")
    rz = get_rotation_around_axis(yaw, "z")
    R = rz @ ry @ rx
    T = np.hstack((R, np.array([[x, y, z]]).reshape((3, 1))))
    transforms.append(T)


pc_list = glob.glob("/media/matthieu/DATA/KITTI/2011_09_28_drive_0047_sync/velodyne_points/data/*.bin")
pc_list = sorted(pc_list)
pc_names = []
for f in pc_list:
    pc_names.append(os.path.splitext(os.path.basename(f))[0])


total_pc = []
for i, pc_file in enumerate(pc_list[:-1]):
    print("pointcloud ", i)
    # load point cloud
    pts = np.fromfile(pc_file, np.float32, -1).reshape((-1, 4))
    pts = pts[:, :3]

    # transform points
    # no transform for this data the car is not moving
    pts = transforms[i][:, :3] @ pts.T + transforms[i][:, 3].reshape((3, 1))
    pts = pts.T
    
    total_pc.append(pts)
    
total_pc = np.vstack(total_pc)

with open("registered_pointclouds.obj", "w") as fout:
    for p in total_pc:
        fout.write("v " + " ".join(p.astype(str)) + "\n")