# Copyright (c) 2022-2023, NVIDIA Corporation & Affiliates. All rights reserved.
#
# This work is made available under the Nvidia Source Code License-NC.
# To view a copy of this license, visit
# https://github.com/NVlabs/VoxFormer/blob/main/LICENSE

#!/usr/bin/env python3
# This file is covered by the LICENSE file in the root of this project.

import argparse
import os

import numpy as np
from tqdm.contrib.concurrent import process_map

from collections import deque
import shutil
from numpy.linalg import inv
import time
import mapping
import torch

import sys 
# import atomic_max_custom
# from scipy.stats import norm


# ray_voxel_intersection_root="/path/tmpmy/ray_voxel_intersection"
# # for i in range(0, 466615):
# ray_voxel_intersection_list=[]
# from tqdm import tqdm
# for i in tqdm(range(0, 466616)):
#   ray_voxel_intersection_file = os.path.join(ray_voxel_intersection_root, str(i).zfill(8) + ".npy")
#   ray_voxel_intersection = np.load(ray_voxel_intersection_file)
#   ray_voxel_intersection_list.append(ray_voxel_intersection)


ray_voxel_intersection_root="/path/VoxFormer_UQ/ray_voxel_intersection/semantickitti/"
import pickle
global ray_voxel_intersection_list
global ray_voxel_intersection_list_ori

ray_voxel_intersection_list_ori=None
ray_voxel_intersection_list=None


a=0
  # print(ray_voxel_intersection)
  # print(np.max(ray_voxel_intersection))
  # print(np.min(ray_voxel_intersection))
  # print(np.mean(ray_voxel_intersection))
  # print(np.std(ray_voxel_intersection))
  # print(caculate_cdf(0, np.mean(ray_voxel_intersection), np.std(ray_voxel_intersection)))
 

def pack(array):
  """ convert a boolean array into a bitwise array. """
  array = array.reshape((-1))

  #compressing bit flags.
  # yapf: disable
  compressed = array[::8] << 7 | array[1::8] << 6  | array[2::8] << 5 | array[3::8] << 4 | array[4::8] << 3 | array[5::8] << 2 | array[6::8] << 1 | array[7::8]
  # yapf: enable

  return np.array(compressed, dtype=np.uint8)

def parse_calibration(filename):
  """ read calibration file with given filename

      Returns
      -------
      dict
          Calibration matrices as 4x4 numpy arrays.
  """
  calib = {}

  calib_file = open(filename)
  for line in calib_file:
    key, content = line.strip().split(":")
    values = [float(v) for v in content.strip().split()]

    pose = np.zeros((4, 4))
    pose[0, 0:4] = values[0:4]
    pose[1, 0:4] = values[4:8]
    pose[2, 0:4] = values[8:12]
    pose[3, 3] = 1.0

    calib[key] = pose

  calib_file.close()

  return calib


def parse_poses(filename, calibration):
  """ read poses file with per-scan poses from given filename

      Returns
      -------
      list
          list of poses as 4x4 numpy arrays.
  """
  file = open(filename)

  poses = []

  Tr = calibration["Tr"]
  Tr_inv = inv(Tr)

  for line in file:
    values = [float(v) for v in line.strip().split()]

    pose = np.zeros((4, 4))
    pose[0, 0:4] = values[0:4]
    pose[1, 0:4] = values[4:8]
    pose[2, 0:4] = values[8:12]
    pose[3, 3] = 1.0

    poses.append(np.matmul(Tr_inv, np.matmul(pose, Tr)))

  return poses

def parallel_work_sequence(f,thr,ops_type):
  global ray_voxel_intersection_list_ori
  # print(1)

  ray_voxel_intersection_list=ray_voxel_intersection_list_ori.clone()

  # read scan and labels, get pose
  scan_filename = os.path.join(input_folder, f)
  scan = np.fromfile(scan_filename, dtype=np.float32)

  scan_uq=np.load(scan_filename.replace("lidar","depth").replace(".bin","_uq.npy"))

  scan = scan.reshape((-1, 4))

  # convert points to homogenous coordinates (x, y, z, 1)
  points = np.ones((scan.shape))
  points[:, 0:3] = scan[:, 0:3]
  # remissions = scan[:, 3]

  # prepare single numpy array for all points that can be written at once.
  num_concat_points = points.shape[0]

  # num_concat_points += sum([past["points"].shape[0] for past in history])
  concated_points = np.zeros((num_concat_points * 4), dtype = np.float32)
  # concated_labels = np.zeros((num_concat_points), dtype = np.uint32)

  start = 0
  concated_points[4 * start:4 * (start + points.shape[0])] = scan.reshape((-1))
  # concated_labels[start:start + points.shape[0]] = labels
  start += points.shape[0]

  if float(os.path.splitext(f)[0])%5==0:
  # # if True:
  #   #------------------------------------------------------------------------------------------------------------------------  
  #   #------------------------------------------------------------------------------------------------------------------------  
  #   # (1). visibility (voxel) generation
  #   visibility_maps = []
  #   # origins = np.zeros((1,4))
  #   origins = np.array([[0, 0, 0, 1]])
  #   map_dims = [256, 256, 32]
    voxel_size = (0.2, 0.2, 0.2)
    area_extents = np.array([[0, 51.2], [-25.6, 25.6], [-2., 4.4]])
    
    # pc_range = [area_extents[0,0], area_extents[1,0], area_extents[2,0], area_extents[0,1], area_extents[1,1], area_extents[2,1]]
    pts = concated_points.reshape(-1,4)
    
    pts=scan.reshape(-1,4)

    filter_idx = np.where((area_extents[0, 0] < pts[:, 0]) & (pts[:, 0] < area_extents[0, 1]) & (area_extents[1, 0] < pts[:, 1]) & (pts[:, 1] < area_extents[1, 1]) & (area_extents[2, 0] < pts[:, 2]) & (pts[:, 2] < area_extents[2, 1]))[0]
    # print(pts.shape)
    pts = pts[filter_idx]
    scan_uq=scan_uq.reshape(-1)[filter_idx]




    # # import numpy as np
    # import open3d as o3d
    # pcd = o3d.geometry.PointCloud()

    # # 将 numpy 数组转换为 Open3D 理解的点云格式
    # pcd.points = o3d.utility.Vector3dVector(pts[:,:3])

    # # 可视化点云
    # o3d.visualization.draw_geometries([pcd])


    output_tensor=np.zeros((256,256,32))

    import torch
    ##########################################################################################################
    output_tensor = torch.zeros((256, 256, 32), device='cuda')
    #to double
    output_tensor=output_tensor.double()
    
    # print(ray_voxel_intersection_list)
    # 将相关数据转移到GPU上

    ray_info = ray_voxel_intersection_list[filter_idx]
    pts = torch.tensor(pts, device='cuda')
    scan_uq = torch.tensor(scan_uq, device='cuda')

    # myst=1000
    # myed=1100
    # ray_info=ray_info[myst:myed]
    # pts=pts[myst:myed]
    # scan_uq=scan_uq[myst:myed]


    
    st = ray_info[:, :3]
    direction = ray_info[:, 3:6]
    # pts_i = pts[torch.arange(len(filter_idx)), :]
    pts_i = pts

    t1 = (pts_i[:, 0] - st[:, 0]) / direction[:, 0]
    # t2 = (pts_i[:, 1] - st[:, 1]) / direction[:, 1]
    # t3 = (pts_i[:, 2] - st[:, 2]) / direction[:, 2]

    # mean_t = torch.stack([t1, t2, t3], dim=1).mean(dim=1)
    mean_t = torch.stack([t1, t1, t1], dim=1).mean(dim=1)
    std_t = torch.exp(scan_uq / 2) / direction[:, 0]

    gaussian_dist = torch.distributions.Normal(loc=mean_t.unsqueeze(1), scale=std_t.unsqueeze(1))
    cdf_values = gaussian_dist.cdf(ray_info[:, 6:])

    # thr=1
    upper_bound = mean_t + thr * std_t
    lower_bound = mean_t - thr * std_t

    # upper_bound最大1
    upper_bound = torch.where(upper_bound > 1, torch.ones_like(upper_bound), upper_bound)
    # lower_bound最小0
    lower_bound = torch.where(lower_bound < 0, torch.zeros_like(lower_bound), lower_bound)

    mask = (ray_info[:, 6:-1] >= lower_bound.unsqueeze(1)) & (ray_info[:, 6:-1] <= upper_bound.unsqueeze(1)) &\
            (ray_info[:, 7:] >= lower_bound.unsqueeze(1)) & (ray_info[:, 7:] <= upper_bound.unsqueeze(1))
    # mask[:, 0] = False

    test_t = torch.where(mask, (ray_info[:, 6:-1] + ray_info[:, 7:]) / 2, torch.zeros_like(ray_info[:, 6:-1]))

    x = st[:, 0:1] + direction[:, 0:1] * test_t
    y = st[:, 1:2] + direction[:, 1:2] * test_t
    z = st[:, 2:3] + direction[:, 2:3] * test_t

    x_index = ((x - area_extents[0, 0]) / voxel_size[0]).long().cuda()
    y_index = ((y - area_extents[1, 0]) / voxel_size[1]).long().cuda()
    z_index = ((z - area_extents[2, 0]) / voxel_size[2]).long().cuda()

    valid_mask = (x_index >= 0) & (x_index < 256) & (y_index >= 0) & (y_index < 256) & (z_index >= 0) & (z_index < 32)

    cdf_diff = cdf_values[:, 1:] - cdf_values[:, :-1] 
    cdf_diff = torch.where(mask[:, :], cdf_diff, torch.zeros_like(cdf_diff))



    if ops_type=="max":
      assert False
      atomic_max_custom.atomic_max(
          output_tensor,
          x_index[valid_mask],
          y_index[valid_mask],
          z_index[valid_mask],
          cdf_diff[valid_mask]
      )

      torch.cuda.synchronize()
    elif ops_type=="sum":
      # output_tensor.index_put_((x_index[valid_mask], y_index[valid_mask], z_index[valid_mask]), cdf_diff[valid_mask], accumulate=True)
      # torch.cuda.synchronize()
      valid_x_index = x_index[valid_mask]
      valid_y_index = y_index[valid_mask]
      valid_z_index = z_index[valid_mask]
      valid_cdf_diff = cdf_diff[valid_mask]
      output_tensor = output_tensor.view(-1)  # 展平 output_tensor
      indices_1D = valid_z_index + valid_y_index * 32 + valid_x_index * 32 * 256
      indices_1D = indices_1D.view(-1)
      output_tensor.scatter_add_(0, indices_1D, valid_cdf_diff)
      output_tensor = output_tensor.view(256, 256, 32)
      torch.cuda.synchronize()
    else:
      raise NotImplementedError(ops_type)

    
    # #存储numpy
    # # print(2)
    output_tensor=output_tensor.cpu()
    # # print(3)
    output_tensor=output_tensor.numpy()
    # # print(4)
    np.save(os.path.join(output_folder, os.path.splitext(f)[0] + ".npy"), output_tensor)
    # # print(5)
    # print("Finished processing:",float(os.path.splitext(f)[0]))

#max



    # print("FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF")
    # ##########################################################################################################


    # # for i_tmp, filter_idx_i in enumerate(filter_idx):
    # # tqdm
    # a=0
    # output_tensor=np.zeros((256,256,32))
    # from tqdm import tqdm
    # for i_tmp, filter_idx_i in enumerate(tqdm(filter_idx)):
    #   ray_info=ray_voxel_intersection_list[filter_idx_i]
    #   st=ray_info[:3]
    #   direction=ray_info[3:6]
    #   pts_i=pts[i_tmp]
    #   t1=(pts_i[0]-st[0])/direction[0]
    #   t2=(pts_i[1]-st[1])/direction[1]
    #   t3=(pts_i[2]-st[2])/direction[2]
      
    #   mean_t=np.array([t1,t2,t3]).mean()

    #   # a=np.exp(scan_uq[i_tmp]/2)
    

    #   #scan_uq应该是存储的2log(sigma)
    #   std_t=np.exp(scan_uq[i_tmp]/2)/direction[0]


    #   # print(t1, t2, t3, pts_i, st)


    #   gaussian_dist = stats.norm(loc=mean_t, scale=std_t)

    #   cdf_values = gaussian_dist.cdf(ray_info[6:])

    #   #+-3sigma
    #   upper_bound=mean_t+3*std_t
    #   lower_bound=mean_t-3*std_t

    #   for i, cdf_value in enumerate(cdf_values):
    #     if ray_info[6+i]<lower_bound:
    #       continue
    #     if ray_info[6+i]>upper_bound:
    #       break

    #     if i==0:
    #       continue
    #       test_t=(ray_info[6+i]+0)/2
    #     else:
    #       test_t=(ray_info[6+i]+ray_info[6+i-1])/2
    #     x=st[0]+direction[0]*test_t
    #     y=st[1]+direction[1]*test_t
    #     z=st[2]+direction[2]*test_t
    #     #向下取整
    #     x_index=int((x-area_extents[0, 0])/voxel_size[0])
    #     y_index=int((y-area_extents[1, 0])/voxel_size[1])
    #     z_index=int((z-area_extents[2, 0])/voxel_size[2])

    #     # if x_index>=256 or y_index>=256 or z_index>=32:
    #     output_tensor[x_index,y_index,z_index]+=cdf_values[i]-cdf_values[i-1]



    # a=0


    #   # output_tensor


    #     # x = ray_start[0] + ray_dir[0] * t
    #     # y = ray_start[1] + ray_dir[1] * t
    #     # z = ray_start[2] + ray_dir[2] * t



    # #   mean=pts[i_tmp]
    # #   if i_tmp==100:
    # #     a=0
      
      
    # visibility_maps.append(mapping.compute_logodds_dp(pts, origins[[0],:3], pc_range, range(pts.shape[0]), 0.2)) #, lo_occupied, lo_free
    # visibility_maps = np.asarray(visibility_maps)
    # visibility_maps = visibility_maps.reshape(-1, map_dims[2], map_dims[0], map_dims[1])
    # visibility_maps = np.swapaxes(visibility_maps,2,3)  # annotate when generating mesh for coordinate issues - > car heading y
    # visibility_maps = np.transpose(visibility_maps,(0,2,3,1))
    
    # vis_occupy_indices = np.asarray(np.where(visibility_maps>0)).astype(np.uint8)
    # vis_free_indices = np.asarray(np.where(visibility_maps<0)).astype(np.uint8)

    # recover = np.zeros_like(visibility_maps, dtype = np.uint8) # for visualizations: uint16; for training: uint8
    # recover[vis_occupy_indices[0,:],vis_occupy_indices[1,:],vis_occupy_indices[2,:],vis_occupy_indices[3,:]] = 1 #math.log(0.7/(1-0.7))
    # recover[vis_free_indices[0,:],vis_free_indices[1,:],vis_free_indices[2,:],vis_free_indices[3,:]] = 0 #math.log(0.4/(1-0.4))


    # # do packing 
    # visibility_map_bin_pack = pack(recover)
    # visibility_map_bin_pack.tofile(os.path.join(output_folder, os.path.splitext(f)[0] + ".pseudo"))

    # # visibility_map_bin = np.array(recover.reshape(-1))
    # # visibility_map_bin.tofile(os.path.join(output_folder, os.path.splitext(f)[0] + ".pseudo"))


    # # print(float(os.path.splitext(f)[0]))
    # # print("Finished processing:",float(os.path.splitext(f)[0]))


if __name__ == '__main__':
  # global ray_voxel_intersection_list
  # global ray_voxel_intersection_list_ori
  start_time = time.time()

  parser = argparse.ArgumentParser("./lidar2voxel.py")
  parser.add_argument(
      '--dataset',
      '-d',
      type=str,
      required=True,
      help='dataset folder containing all sequences in a folder called "sequences".',
  )

  parser.add_argument(
      '--output',
      '-o',
      type=str,
      required=True,
      help='output folder for generated sequence scans.',
  )
  parser.add_argument(
      '--num_seq',
      '-n',
      type=str,
      required=True,
      help='number of sequence',
  )

  parser.add_argument(
      '--model',
      '-m',
      type=str,
      default='msnet3dmax1std',
      help='depth model',
  )

  parser.add_argument(
      '--sequence_length',
      '-s',
      type=int,
      default=1,
      help='length of sequence, i.e., how many scans are concatenated.',
  )

  
  FLAGS, unparsed = parser.parse_known_args()
  dataset = FLAGS.dataset
  output = FLAGS.output
  num_seq = FLAGS.num_seq

  print("*" * 80)
  print(" dataset folder: ", FLAGS.dataset)
  print("  output folder: ", FLAGS.output)
  print("sequence length: ", FLAGS.sequence_length)
  print("*" * 80)


  with open(os.path.join(ray_voxel_intersection_root,str(num_seq).zfill(2)+'_array_list.pkl') , 'rb') as f:

  # with open(os.path.join(ray_voxel_intersection_root,'array_list.pkl') , 'rb') as f:

  
    ray_voxel_intersection_list = pickle.load(f)
    ray_voxel_intersection_list = [item + [item[-1]] + [100] for item in ray_voxel_intersection_list]
    ray_voxel_intersection_list_np = np.array(ray_voxel_intersection_list, dtype=object)
    max_len = max(len(sublist) for sublist in ray_voxel_intersection_list_np)+2
    ray_voxel_intersection_list = np.array([np.pad(sublist, (0, max_len - len(sublist)), mode='constant', constant_values=1) for sublist in ray_voxel_intersection_list_np])
    ray_voxel_intersection_list_ori = torch.tensor(ray_voxel_intersection_list, device='cuda')
    ray_voxel_intersection_list=None
    a=0



  if FLAGS.sequence_length==1:

    sequences_dir = os.path.join(dataset, "sequences")
    input_folder = os.path.join(sequences_dir, num_seq)

    pseudo_lidar_files = [
          f for f in sorted(os.listdir(input_folder))
          if f.endswith(".bin")
      ]
    # output_folder = os.path.join(output, "sequences", num_seq, "voxels")
    # output_folder = os.path.join(output, "sequences_msnet3d_sweep1", num_seq, "voxels")
    # output_folder = os.path.join(output, "sequences_msnet3d_sweep1", num_seq, "voxels")
    output_folder = os.path.join(output, "sequences_" + FLAGS.model + "_sweep"+str(FLAGS.sequence_length), num_seq, "voxels")



    # mesh_temp_folder = os.path.join(output_folder, "temp")
    # mesh_folder = os.path.join(output_folder, "mesh")
    # scan_folder = os.path.join(output, "sequences", str(num_scan+1) + num_seq, "voxels")

    if not os.path.exists(output_folder):
      os.makedirs(output_folder)

    # process_map(parallel_work_sequence, pseudo_lidar_files, max_workers=18)

    from tqdm import tqdm
    # for f in pseudo_lidar_files:
    ops_type=None
    thr=None
    if "max" in FLAGS.model:
      ops_type="max"
    elif "sum" in FLAGS.model:
      ops_type="sum"
    else:
      raise NotImplementedError(FLAGS.model)

      # msnet3dmax1std
    thr=int(FLAGS.model[-4])

    for f in tqdm(pseudo_lidar_files):
      parallel_work_sequence(f,thr,ops_type)
    # parallel_work_sequence(pseudo_lidar_files[0])
      torch.cuda.empty_cache()
    print("finished.")
    print("execution time: {}".format(time.time() - start_time))
  
  else:
    assert False


    folder=str(num_seq)
    sequences_dir = os.path.join(FLAGS.dataset, "sequences")
    sequence_folders = [
        f for f in sorted(os.listdir(sequences_dir))
        if os.path.isdir(os.path.join(sequences_dir, f))
    ]

    # for folder in ["19"]:
    input_folder = os.path.join(sequences_dir, num_seq)
    output_folder = os.path.join(FLAGS.output, "sequences_" + FLAGS.model + "_sweep"+str(FLAGS.sequence_length), folder)

    scan_files = [
        f for f in sorted(os.listdir(os.path.join(input_folder)))
        if f.endswith(".bin")
    ]

    history = deque()

    calibration = parse_calibration(os.path.join(input_folder, "calib.txt"))
    poses = parse_poses(os.path.join(input_folder, "poses.txt"), calibration)

    progress = 10

    print("Processing {} ".format(folder), end="", flush=True)


    # raise NotImplementedError(scan_files)

    for i, f in enumerate(scan_files):
      # read scan and labels, get pose
      scan_filename = os.path.join(input_folder, f)
      scan = np.fromfile(scan_filename, dtype=np.float32)

      scan = scan.reshape((-1, 4))

      # label_filename = os.path.join(input_folder, "labels", os.path.splitext(f)[0] + ".label")
      # labels = np.fromfile(label_filename, dtype=np.uint32)
      # labels = labels.reshape((-1))

      # convert points to homogenous coordinates (x, y, z, 1)
      points = np.ones((scan.shape))
      points[:, 0:3] = scan[:, 0:3]
      remissions = scan[:, 3]

      pose = poses[i]

      # prepare single numpy array for all points that can be written at once.
      num_concat_points = points.shape[0]
      num_concat_points += sum([past["points"].shape[0] for past in history])
      concated_points = np.zeros((num_concat_points * 4), dtype = np.float32)
      # concated_labels = np.zeros((num_concat_points), dtype = np.uint32)

      start = 0
      concated_points[4 * start:4 * (start + points.shape[0])] = scan.reshape((-1))
      # concated_labels[start:start + points.shape[0]] = labels
      start += points.shape[0]

      for past in history:
        diff = np.matmul(inv(pose), past["pose"])
        tpoints = np.matmul(diff, past["points"].T).T
        tpoints[:, 3] = past["remissions"]
        tpoints = tpoints.reshape((-1))

        concated_points[4 * start:4 * (start + past["points"].shape[0])] = tpoints
        # concated_labels[start:start + past["labels"].shape[0]] = past["labels"]
        start += past["points"].shape[0]

      if float(os.path.splitext(f)[0])%5==0:
        print(float(os.path.splitext(f)[0]))
      # if True:
        #------------------------------------------------------------------------------------------------------------------------  
        #------------------------------------------------------------------------------------------------------------------------  
        # (1). visibility (voxel) generation
        visibility_maps = []
        # origins = np.zeros((1,4))
        origins = np.array([[0, 0, 0, 1]])
        map_dims = [256, 256, 32]
        voxel_size = (0.2, 0.2, 0.2)
        area_extents = np.array([[0, 51.2], [-25.6, 25.6], [-2., 4.4]])
        
        pc_range = [area_extents[0,0], area_extents[1,0], area_extents[2,0], area_extents[0,1], area_extents[1,1], area_extents[2,1]]
        pts = concated_points.reshape(-1,4)
        filter_idx = np.where((area_extents[0, 0] < pts[:, 0]) & (pts[:, 0] < area_extents[0, 1]) & (area_extents[1, 0] < pts[:, 1]) & (pts[:, 1] < area_extents[1, 1]) & (area_extents[2, 0] < pts[:, 2]) & (pts[:, 2] < area_extents[2, 1]))[0]
        pts = pts[filter_idx]


          




        visibility_maps.append(mapping.compute_logodds_dp(pts, origins[[0],:3], pc_range, range(pts.shape[0]), 0.2)) #, lo_occupied, lo_free
        visibility_maps = np.asarray(visibility_maps)
        visibility_maps = visibility_maps.reshape(-1, map_dims[2], map_dims[0], map_dims[1])
        visibility_maps = np.swapaxes(visibility_maps,2,3)  # annotate when generating mesh for coordinate issues - > car heading y
        visibility_maps = np.transpose(visibility_maps,(0,2,3,1))
        
        vis_occupy_indices = np.asarray(np.where(visibility_maps>0)).astype(np.uint8)
        vis_free_indices = np.asarray(np.where(visibility_maps<0)).astype(np.uint8)

        recover = np.zeros_like(visibility_maps, dtype = np.uint8) # for visualizations: uint16; for training: uint8
        recover[vis_occupy_indices[0,:],vis_occupy_indices[1,:],vis_occupy_indices[2,:],vis_occupy_indices[3,:]] = 1 #math.log(0.7/(1-0.7))
        recover[vis_free_indices[0,:],vis_free_indices[1,:],vis_free_indices[2,:],vis_free_indices[3,:]] = 0 #math.log(0.4/(1-0.4))

        # visibility_map_bin = np.array(recover.reshape(-1))
        visibility_map_bin = pack(recover)





        voxel_output_folder = os.path.join(output_folder, "voxels")
        if not os.path.exists(voxel_output_folder):
          os.makedirs(voxel_output_folder)

        visibility_map_bin.tofile(os.path.join(voxel_output_folder, os.path.splitext(f)[0] + ".pseudo"))
        # print(float(os.path.splitext(f)[0]))

        # print("Finished processing:",float(os.path.splitext(f)[0]))

      # append current data to history queue.
      history.appendleft({
          "points": points,
          "remissions": remissions,
          "pose": pose.copy()
      })

      if len(history) >= FLAGS.sequence_length:
        history.pop()


      if 100.0 * i / len(scan_files) >= progress:
        print(".", end="", flush=True)
        progress = progress + 10
    print("finished.")


  print("execution time: {}".format(time.time() - start_time))








# # Copyright (c) 2022-2023, NVIDIA Corporation & Affiliates. All rights reserved.
# #
# # This work is made available under the Nvidia Source Code License-NC.
# # To view a copy of this license, visit
# # https://github.com/NVlabs/VoxFormer/blob/main/LICENSE

# #!/usr/bin/env python3
# # This file is covered by the LICENSE file in the root of this project.

# import argparse
# import os

# import numpy as np
# from tqdm.contrib.concurrent import process_map

# from collections import deque
# import shutil
# from numpy.linalg import inv
# import struct
# import time
# import mapping
 

# def pack(array):
#   """ convert a boolean array into a bitwise array. """
#   array = array.reshape((-1))

#   #compressing bit flags.
#   # yapf: disable
#   compressed = array[::8] << 7 | array[1::8] << 6  | array[2::8] << 5 | array[3::8] << 4 | array[4::8] << 3 | array[5::8] << 2 | array[6::8] << 1 | array[7::8]
#   # yapf: enable

#   return np.array(compressed, dtype=np.uint8)

# def parse_calibration(filename):
#   """ read calibration file with given filename

#       Returns
#       -------
#       dict
#           Calibration matrices as 4x4 numpy arrays.
#   """
#   calib = {}

#   calib_file = open(filename)
#   for line in calib_file:
#     key, content = line.strip().split(":")
#     values = [float(v) for v in content.strip().split()]

#     pose = np.zeros((4, 4))
#     pose[0, 0:4] = values[0:4]
#     pose[1, 0:4] = values[4:8]
#     pose[2, 0:4] = values[8:12]
#     pose[3, 3] = 1.0

#     calib[key] = pose

#   calib_file.close()

#   return calib


# def parse_poses(filename, calibration):
#   """ read poses file with per-scan poses from given filename

#       Returns
#       -------
#       list
#           list of poses as 4x4 numpy arrays.
#   """
#   file = open(filename)

#   poses = []

#   Tr = calibration["Tr"]
#   Tr_inv = inv(Tr)

#   for line in file:
#     values = [float(v) for v in line.strip().split()]

#     pose = np.zeros((4, 4))
#     pose[0, 0:4] = values[0:4]
#     pose[1, 0:4] = values[4:8]
#     pose[2, 0:4] = values[8:12]
#     pose[3, 3] = 1.0

#     poses.append(np.matmul(Tr_inv, np.matmul(pose, Tr)))

#   return poses

# def parallel_work_sequence(f):

#   # read scan and labels, get pose
#   scan_filename = os.path.join(input_folder, f)
#   scan = np.fromfile(scan_filename, dtype=np.float32)

#   scan = scan.reshape((-1, 4))

#   # convert points to homogenous coordinates (x, y, z, 1)
#   points = np.ones((scan.shape))
#   points[:, 0:3] = scan[:, 0:3]
#   remissions = scan[:, 3]

#   # prepare single numpy array for all points that can be written at once.
#   num_concat_points = points.shape[0]

#   # num_concat_points += sum([past["points"].shape[0] for past in history])
#   concated_points = np.zeros((num_concat_points * 4), dtype = np.float32)
#   # concated_labels = np.zeros((num_concat_points), dtype = np.uint32)

#   start = 0
#   concated_points[4 * start:4 * (start + points.shape[0])] = scan.reshape((-1))
#   # concated_labels[start:start + points.shape[0]] = labels
#   start += points.shape[0]

#   if float(os.path.splitext(f)[0])%5==0:
#   # if True:
#     #------------------------------------------------------------------------------------------------------------------------  
#     #------------------------------------------------------------------------------------------------------------------------  
#     # (1). visibility (voxel) generation
#     visibility_maps = []
#     # origins = np.zeros((1,4))
#     origins = np.array([[0, 0, 0, 1]])
#     map_dims = [256, 256, 32]
#     voxel_size = (0.2, 0.2, 0.2)
#     area_extents = np.array([[0, 51.2], [-25.6, 25.6], [-2., 4.4]])
    
#     pc_range = [area_extents[0,0], area_extents[1,0], area_extents[2,0], area_extents[0,1], area_extents[1,1], area_extents[2,1]]
#     pts = concated_points.reshape(-1,4)
#     filter_idx = np.where((area_extents[0, 0] < pts[:, 0]) & (pts[:, 0] < area_extents[0, 1]) & (area_extents[1, 0] < pts[:, 1]) & (pts[:, 1] < area_extents[1, 1]) & (area_extents[2, 0] < pts[:, 2]) & (pts[:, 2] < area_extents[2, 1]))[0]
#     pts = pts[filter_idx]
      
#     visibility_maps.append(mapping.compute_logodds_dp(pts, origins[[0],:3], pc_range, range(pts.shape[0]), 0.2)) #, lo_occupied, lo_free
#     visibility_maps = np.asarray(visibility_maps)
#     visibility_maps = visibility_maps.reshape(-1, map_dims[2], map_dims[0], map_dims[1])
#     visibility_maps = np.swapaxes(visibility_maps,2,3)  # annotate when generating mesh for coordinate issues - > car heading y
#     visibility_maps = np.transpose(visibility_maps,(0,2,3,1))
    
#     vis_occupy_indices = np.asarray(np.where(visibility_maps>0)).astype(np.uint8)
#     vis_free_indices = np.asarray(np.where(visibility_maps<0)).astype(np.uint8)

#     recover = np.zeros_like(visibility_maps, dtype = np.uint8) # for visualizations: uint16; for training: uint8
#     recover[vis_occupy_indices[0,:],vis_occupy_indices[1,:],vis_occupy_indices[2,:],vis_occupy_indices[3,:]] = 1 #math.log(0.7/(1-0.7))
#     recover[vis_free_indices[0,:],vis_free_indices[1,:],vis_free_indices[2,:],vis_free_indices[3,:]] = 0 #math.log(0.4/(1-0.4))


#     # do packing 
#     visibility_map_bin_pack = pack(recover)
#     visibility_map_bin_pack.tofile(os.path.join(output_folder, os.path.splitext(f)[0] + ".pseudo"))

#     # visibility_map_bin = np.array(recover.reshape(-1))
#     # visibility_map_bin.tofile(os.path.join(output_folder, os.path.splitext(f)[0] + ".pseudo"))


#     # print(float(os.path.splitext(f)[0]))
#     # print("Finished processing:",float(os.path.splitext(f)[0]))


# if __name__ == '__main__':
#   start_time = time.time()

#   parser = argparse.ArgumentParser("./lidar2voxel.py")
#   parser.add_argument(
#       '--dataset',
#       '-d',
#       type=str,
#       required=True,
#       help='dataset folder containing all sequences in a folder called "sequences".',
#   )

#   parser.add_argument(
#       '--output',
#       '-o',
#       type=str,
#       required=True,
#       help='output folder for generated sequence scans.',
#   )
#   parser.add_argument(
#       '--num_seq',
#       '-n',
#       type=str,
#       required=True,
#       help='number of sequence',
#   )

#   parser.add_argument(
#       '--model',
#       '-m',
#       type=str,
#       default='msnet3d',
#       help='depth model',
#   )

#   parser.add_argument(
#       '--sequence_length',
#       '-s',
#       type=int,
#       default=1,
#       help='length of sequence, i.e., how many scans are concatenated.',
#   )

  
#   FLAGS, unparsed = parser.parse_known_args()
#   dataset = FLAGS.dataset
#   output = FLAGS.output
#   num_seq = FLAGS.num_seq

#   print("*" * 80)
#   print(" dataset folder: ", FLAGS.dataset)
#   print("  output folder: ", FLAGS.output)
#   print("sequence length: ", FLAGS.sequence_length)
#   print("*" * 80)


#   if FLAGS.sequence_length==1:
#     sequences_dir = os.path.join(dataset, "sequences")
#     input_folder = os.path.join(sequences_dir, num_seq)

#     pseudo_lidar_files = [
#           f for f in sorted(os.listdir(input_folder))
#           if f.endswith(".bin")
#       ]
#     # output_folder = os.path.join(output, "sequences", num_seq, "voxels")
#     # output_folder = os.path.join(output, "sequences_msnet3d_sweep1", num_seq, "voxels")
#     # output_folder = os.path.join(output, "sequences_msnet3d_sweep1", num_seq, "voxels")
#     output_folder = os.path.join(output, "sequences_" + FLAGS.model + "_sweep"+str(FLAGS.sequence_length), num_seq, "voxels")

#     # mesh_temp_folder = os.path.join(output_folder, "temp")
#     # mesh_folder = os.path.join(output_folder, "mesh")
#     # scan_folder = os.path.join(output, "sequences", str(num_scan+1) + num_seq, "voxels")

#     if not os.path.exists(output_folder):
#       os.makedirs(output_folder)

#     process_map(parallel_work_sequence, pseudo_lidar_files, max_workers=18)

#     print("finished.")
#     print("execution time: {}".format(time.time() - start_time))
  
#   else:
#     folder=str(num_seq)
#     sequences_dir = os.path.join(FLAGS.dataset, "sequences")
#     sequence_folders = [
#         f for f in sorted(os.listdir(sequences_dir))
#         if os.path.isdir(os.path.join(sequences_dir, f))
#     ]

#     # for folder in ["19"]:
#     input_folder = os.path.join(sequences_dir, num_seq)
#     output_folder = os.path.join(FLAGS.output, "sequences_" + FLAGS.model + "_sweep"+str(FLAGS.sequence_length), folder)

#     scan_files = [
#         f for f in sorted(os.listdir(os.path.join(input_folder)))
#         if f.endswith(".bin")
#     ]

#     history = deque()

#     calibration = parse_calibration(os.path.join(input_folder, "calib.txt"))
#     poses = parse_poses(os.path.join(input_folder, "poses.txt"), calibration)

#     progress = 10

#     print("Processing {} ".format(folder), end="", flush=True)

#     for i, f in enumerate(scan_files):
#       # read scan and labels, get pose
#       scan_filename = os.path.join(input_folder, f)
#       scan = np.fromfile(scan_filename, dtype=np.float32)

#       scan = scan.reshape((-1, 4))

#       # label_filename = os.path.join(input_folder, "labels", os.path.splitext(f)[0] + ".label")
#       # labels = np.fromfile(label_filename, dtype=np.uint32)
#       # labels = labels.reshape((-1))

#       # convert points to homogenous coordinates (x, y, z, 1)
#       points = np.ones((scan.shape))
#       points[:, 0:3] = scan[:, 0:3]
#       remissions = scan[:, 3]

#       pose = poses[i]

#       # prepare single numpy array for all points that can be written at once.
#       num_concat_points = points.shape[0]
#       num_concat_points += sum([past["points"].shape[0] for past in history])
#       concated_points = np.zeros((num_concat_points * 4), dtype = np.float32)
#       # concated_labels = np.zeros((num_concat_points), dtype = np.uint32)

#       start = 0
#       concated_points[4 * start:4 * (start + points.shape[0])] = scan.reshape((-1))
#       # concated_labels[start:start + points.shape[0]] = labels
#       start += points.shape[0]

#       for past in history:
#         diff = np.matmul(inv(pose), past["pose"])
#         tpoints = np.matmul(diff, past["points"].T).T
#         tpoints[:, 3] = past["remissions"]
#         tpoints = tpoints.reshape((-1))

#         concated_points[4 * start:4 * (start + past["points"].shape[0])] = tpoints
#         # concated_labels[start:start + past["labels"].shape[0]] = past["labels"]
#         start += past["points"].shape[0]

#       if float(os.path.splitext(f)[0])%5==0:
#         print(float(os.path.splitext(f)[0]))
#       # if True:
#         #------------------------------------------------------------------------------------------------------------------------  
#         #------------------------------------------------------------------------------------------------------------------------  
#         # (1). visibility (voxel) generation
#         visibility_maps = []
#         # origins = np.zeros((1,4))
#         origins = np.array([[0, 0, 0, 1]])
#         map_dims = [256, 256, 32]
#         voxel_size = (0.2, 0.2, 0.2)
#         area_extents = np.array([[0, 51.2], [-25.6, 25.6], [-2., 4.4]])
        
#         pc_range = [area_extents[0,0], area_extents[1,0], area_extents[2,0], area_extents[0,1], area_extents[1,1], area_extents[2,1]]
#         pts = concated_points.reshape(-1,4)
#         filter_idx = np.where((area_extents[0, 0] < pts[:, 0]) & (pts[:, 0] < area_extents[0, 1]) & (area_extents[1, 0] < pts[:, 1]) & (pts[:, 1] < area_extents[1, 1]) & (area_extents[2, 0] < pts[:, 2]) & (pts[:, 2] < area_extents[2, 1]))[0]
#         pts = pts[filter_idx]
          
#         visibility_maps.append(mapping.compute_logodds_dp(pts, origins[[0],:3], pc_range, range(pts.shape[0]), 0.2)) #, lo_occupied, lo_free
#         visibility_maps = np.asarray(visibility_maps)
#         visibility_maps = visibility_maps.reshape(-1, map_dims[2], map_dims[0], map_dims[1])
#         visibility_maps = np.swapaxes(visibility_maps,2,3)  # annotate when generating mesh for coordinate issues - > car heading y
#         visibility_maps = np.transpose(visibility_maps,(0,2,3,1))
        
#         vis_occupy_indices = np.asarray(np.where(visibility_maps>0)).astype(np.uint8)
#         vis_free_indices = np.asarray(np.where(visibility_maps<0)).astype(np.uint8)

#         recover = np.zeros_like(visibility_maps, dtype = np.uint8) # for visualizations: uint16; for training: uint8
#         recover[vis_occupy_indices[0,:],vis_occupy_indices[1,:],vis_occupy_indices[2,:],vis_occupy_indices[3,:]] = 1 #math.log(0.7/(1-0.7))
#         recover[vis_free_indices[0,:],vis_free_indices[1,:],vis_free_indices[2,:],vis_free_indices[3,:]] = 0 #math.log(0.4/(1-0.4))

#         # visibility_map_bin = np.array(recover.reshape(-1))
#         visibility_map_bin = pack(recover)

#         voxel_output_folder = os.path.join(output_folder, "voxels")
#         if not os.path.exists(voxel_output_folder):
#           os.makedirs(voxel_output_folder)

#         visibility_map_bin.tofile(os.path.join(voxel_output_folder, os.path.splitext(f)[0] + ".pseudo"))
#         # print(float(os.path.splitext(f)[0]))

#         # print("Finished processing:",float(os.path.splitext(f)[0]))

#       # append current data to history queue.
#       history.appendleft({
#           "points": points,
#           "remissions": remissions,
#           "pose": pose.copy()
#       })

#       if len(history) >= FLAGS.sequence_length:
#         history.pop()

#       if 100.0 * i / len(scan_files) >= progress:
#         print(".", end="", flush=True)
#         progress = progress + 10
#     print("finished.")


#   print("execution time: {}".format(time.time() - start_time))

