import socket
import struct
import pyrealsense2 as rs
import cv2
import numpy as np
import mediapipe as mp
from time import sleep

# Utility functions

def mapValue(value, from_low, from_high, to_low, to_high):
    return (value - from_low) * (to_high - to_low) / (from_high - from_low) + to_low

def getPose():
    #Establishes a connection with the UR5 robot,
    #receives its current pose (position and orientation), 
    #and returns this information

def sendInstructionToUr5(ip, port, instruction):
    #Sends instructions to the UR5 robot using a specified IP address and port.

def startCamera():
    #Initializes the RealSense camera and displays real-time video streams.

def frameOriginCoordinates(xtool, ytool, H, V, wrist3):
    #Calculates the origin coordinates of the camera frame 
    #based on the robot's position and orientation.    

def transformCoordinates(x1, y1, ofx, ofy, theta):
    #Transforms coordinates from the camera frame to global coordinates.

def showAndGetPredictionsLive():
    # Captures real-time frames from the camera,
    #detects hands using MediaPipe, and returns hand-related information.

#Main loop
#Transforms coordinates

#Sends instruction to UR seccuentially
    resp1 = int(input("Move to hand: "))
    if resp1 == 1:
        ur5_move_command = f"movel(p[{xtransf1},{ytransf1},{ofzr},{rxr},{ryr},{rzr}],
         a = 1.2, v = 0.25, t = 0, r = 0)\n"
        sendInstructionToUr5(ur5_ip, ur5_port, ur5_move_command)
    sleep(3)
    resp2 = int(input("Press 1 to return to original pose: "))
    if resp2 == 1:
        ur5_move_command_1 = "movel(p[.117,-.364,.375,0,3.142,0], 
         a = 1.2, v = 0.25, t = 0, r = 0)\n"
        sendInstructionToUr5(ur5_ip, ur5_port, ur5_move_command_1)
    sleep(3)