import SDK.Vzense.VzenseDS_api as tof
import etc_pb2 as etc
import time
from ctypes import *


class Tof3DDevice:
    def __init__(self, tof_etc=etc.VzenseTofDevices()):
        self.tof_etc = tof_etc
        print(self.tof_etc.ipv4)
        camera = tof.VzenseTofCam()
        camera_count = camera.VZ_GetDeviceCount()
        retry_count = 100
        while camera_count == 0 and retry_count > 0:
            retry_count = retry_count - 1
            camera_count = camera.VZ_GetDeviceCount()
            time.sleep(1)
            print("scaning......   ", retry_count)

        device_info = tof.VzDeviceInfo()

        if camera_count > 1:
            ret, device_infolist = camera.VZ_GetDeviceInfoList(camera_count)
            if ret == 0:
                device_info = device_infolist[0]
                for info in device_infolist:
                    print('cam uri:  ' + str(info.uri))
            else:
                print(' failed:' + ret)
                exit()
        elif camera_count == 1:
            ret, device_info = camera.VZ_GetDeviceInfo()
            if ret == 0:
                print('cam uri:' + str(device_info.uri))
            else:
                print(' failed:' + ret)
                exit()
        else:
            print("there are no camera found")
            exit()

        if tof.VzConnectStatus.Connected.value != device_info.status:
            print("connect statu:", device_info.status)
            print("Call VZ_OpenDeviceByUri with connect status :", tof.VzConnectStatus.Connected.value)
            exit()
        else:
            print("uri: " + str(device_info.uri))
            print("alias: " + str(device_info.alias))
            print("connectStatus: " + str(device_info.status))

        print(self.tof_etc.ipv4)
        # ret = camera.VZ_OpenDeviceByIP(b'192.168.2.101')
        ret = camera.VZ_OpenDeviceByIP(self.tof_etc.ipv4.encode())

        if ret == 0:
            print("open device successful")
        else:
            print('VZ_OpenDeviceByUri failed: ' + str(ret))

        ret = camera.VZ_StartStream()
        if ret == 0:
            print("start stream successful")
        else:
            print("VZ_StartStream failed:", ret)