import os
import sys
import pyzed.sl as sl
import json
import numpy as np 
import math 
import time
import subprocess
import mailcalibrate

WDIR_DEFAULT = os.path.expanduser('~/data/')
SDIR_DEFAULT = os.path.abspath('/hdd/')
MAINCAM_DEFAULT = 'cam1'
RECFRAME_DEFAULT = 1800
JSON_PATH = '~/sources/configuration.json'
LOG_REBOOT = '~/sources/cfg/log_reboot.txt'

cameras = sl.Camera.get_device_list()

def get_cam_info():
    init = sl.InitParameters()
    init.depth_mode = sl.DEPTH_MODE.NONE  # Use PERFORMANCE depth mode
    init.coordinate_units = sl.UNIT.METER  # Use meter units (for depth measurements)
    init.camera_resolution = sl.RESOLUTION.HD720
    name_list = []
    
    zed_list = []
    angle_list = []    

    for cam in cameras:        
        init.set_from_serial_number(cam.serial_number)
        name_list.append("ZED {}".format(cam.serial_number))
        zed = sl.Camera()
        err = zed.open(init)
        if err != sl.ERROR_CODE.SUCCESS:
            print(repr(err))
            zed.close()
        info = zed.get_camera_information()
        sensors_data = sl.SensorsData()
        if zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.CURRENT) == sl.ERROR_CODE.SUCCESS :
            #print(" - IMU:")
            # Filtered orientation quaternion
            quaternion = sensors_data.get_imu_data().get_pose().get_orientation().get()            
            #NOTE: IS IT REALLY CORRECT ANGLE???
            angle = (quaternion[0] * 900)/7+ 90
            angle_list.append(angle)
        print(angle)
        
    zed.close()
    return_list = []
    for x in range(0,len(name_list)):
        return_list.append(name_list[x])
        return_list.append(angle_list[x])

    return return_list
    

def main():
    #Initialize JSON
    conf = {
    "confDate" : "",
    "branch" : "",
    "room" : "",
    "workingDir" : "",
    "storageDir" : "",
    "mainCam" : "cam1",
    "recFrame" : 1800,
    "cam1" : {
        "SN" : "",
        "angle" : "",
        "d_center" : "",
        "headCoord" : "",
        "d_headCoord" : "",
        "tailCoord" : "",
        "d_tailCoord" : ""
        },
    "cam2" : {
        "SN" : "",
        "angle" : "",
        "d_center" : "",
        "headCoord" : "",
        "d_headCoord" : "",
        "tailCoord" : "",
        "d_tailCoord" : ""
        }
    }

    wdirpath = os.path.abspath(os.path.expanduser('~/data'))
    sdirpath = os.path.abspath('/hdd')
        
    nowtime = time.localtime()
    #NOTE: createdDate = "2022/02/04 21:07:54"
    createdDate = "%04d/%02d/%02d %02d:%02d:%02d" % (nowtime.tm_year, nowtime.tm_mon, nowtime.tm_mday, nowtime.tm_hour, nowtime.tm_min, nowtime.tm_sec)

    caminfo_list = get_cam_info()
    
    conf["confDate"] = createdDate
    conf["workingDir"] = wdirpath
    conf["storageDir"] = sdirpath
    conf["mainCam"] = caminfo_list[0]
    conf["recFrame"] = RECFRAME_DEFAULT
    conf["cam1"]["SN"] = caminfo_list[0]
    conf["cam1"]["angle"] = caminfo_list[1]
    conf["cam1"]["d_center"] = 0
    conf["cam1"]["headCoord"] = ''
    conf["cam1"]["d_headCoord"] = 0
    conf["cam1"]["tailCoord"] = ''
    conf["cam1"]["d_tailCoord"] = 0
    if len(caminfo_list) == 4:
        conf["mainCam"] = "SN 00000000"
        conf["cam2"]["SN"] = caminfo_list[2]
        conf["cam2"]["angle"] = caminfo_list[3]
        conf["cam2"]["d_center"] = 0
        conf["cam2"]["headCoord"] = ''
        conf["cam2"]["d_headCoord"] = 0
        conf["cam2"]["tailCoord"] = ''
        conf["cam2"]["d_tailCoord"] = 0
    else:
        pass

    if os.path.isfile(os.path.expanduser(JSON_PATH)):
        subprocess.call(('mv', os.path.expanduser(JSON_PATH), os.path.expanduser('~/sources/configuration.json.bak')))
        print('Already configuration file exists.')
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file:
            json.dump(conf,json_file, indent=4)
        print('configuration.json was created')
    else:
        with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file:
            json.dump(conf,json_file, indent=4)

    if os.path.isfile(os.path.expanduser(LOG_REBOOT)) == False:        
        with open (os.path.abspath(os.path.expanduser(LOG_REBOOT)), "w+") as log_reboot:
            log_reboot.write('This file automatically created at '+ createdDate)
            print('log_reboot.txt was created')

if __name__ == "__main__":
    main()