import json
import os

import cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D  # <--- This is important for 3d plotting

import numpy as np
import h5py
import scipy.io
import sys
from pathlib import Path

sys.path.append(str(Path("../").resolve()))

from utils.projection import projectToCamera
from utils.links import joints_mpi_inf_3dhp, links_mpi_inf_3dhp


def main():
    file_name = "/home/daniela/catkin_ws/src/hpe/images/mpi-inf-3dhp/S1/Seq1/annot.mat"
    file_name_calibration = "/home/daniela/catkin_ws/src/hpe/images/mpi-inf-3dhp/S1/Seq1/camera.calibration"

    cameras = {}
    cameras_list = ['camera_0', 'camera_1', 'camera_2', 'camera_4', 'camera_5', 'camera_6', 'camera_7', 'camera_8']

    with open(file_name_calibration, 'r') as calib:
        name = ''
        for line in calib.readlines():
            if line[0:4] == 'name':
                name = int(line[11:])
                cameras[name] = {}
            if line[2:11] == 'intrinsic':
                intrinsics = line[11:].split()
                # print(np.reshape([float(x) for x in intrinsics], (4, 4)))
                cameras[name]['intrinsics'] = np.reshape([float(x) for x in intrinsics], (4, 4))
            if line[2:11] == 'extrinsic':
                intrinsics = line[11:].split()
                cameras[name]['extrinsics'] = np.reshape([float(x) for x in intrinsics], (4, 4))
            if line[2:6] == 'size':
                height, width = line[11:].split()
                cameras[name]['height'] = int(height)
                cameras[name]['width'] = int(width)

    # exit(0)

    mat = scipy.io.loadmat(file_name)
    gt_2d = mat['annot2']

    camera_id = 0
    ground_truth = {}

    # print(camera)


    for camera_idx in range(0,13):
        camera = gt_2d[camera_idx][0]

        camera_name = 'camera_' + str(camera_idx)
        all_keypoints = []
        for frame in camera:
            keypoints_per_frame = []
            for n in range(0, len(frame), 2):
                keypoint = [frame[n], frame[n + 1], 1]
                keypoints_per_frame.append(keypoint)

            all_keypoints.append(keypoints_per_frame)

        if os.path.exists('/home/daniela/catkin_ws/src/hpe/images/mpi-inf-3dhp/S1/Seq1/cache/' + camera_name+'/'):
            output_path = '/home/daniela/catkin_ws/src/hpe/images/mpi-inf-3dhp/S1/Seq1/cache/' + camera_name + '/2d_pose_gt.npy'
            np.save(output_path, all_keypoints, allow_pickle=True)
            print('Saved file with keypoint values with success!')



    # with open(output_path, "w") as fp:
    #     # Load the dictionary from the file
    #     json.dump(ground_truth, fp)
    #     print("Ground truth file saved successfully!")


if __name__ == "__main__":
    main()