import numpy as np
from PaIRS_UniNa.PaIRS_PIV import MappingFunction

# Create mapping function object
mapFun = MappingFunction()

# List of calibration files (one per camera)
calFileName = ['calib\pyCal1.cal', 'calib\pyCal2.cal']

# Load calibration data from files
mapFun.readCal(calFileName)  # readCal loads the calibration parameters

numPoints = 4  # Number of points to be mapped

# Select numerical precision: double (np.float64) or single (np.float32, faster)
type = np.float64 

# Define points in the world reference system
# Here we create numPoints points with small variations along the coordinates
points = np.array([[1,2,3]] * numPoints, dtype=type, order='C') + \
         np.array(np.arange(numPoints) * 0.001, dtype=type)[:, None]

# Select the camera index (starting from 0)
# If cam == -1, the mapping is computed for all cameras
cam = -1  

# Output array (can be preallocated or set to None)
X = None  

# Map world points to image coordinates
X1 = mapFun.worldToImg(points, cam, None)

# If X is correctly preallocated, X1 will reference the same memory as X
print(f'id(X)={id(X)} id(X1)={id(X1)} same Id={id(X)==id(X1)}')

# Repeat the same operation using single precision
type = np.float32

# Define world points again with the new precision
points = np.array([[1,2,3]] * numPoints, dtype=type, order='C') + \
         np.array(np.arange(numPoints) * 0.001, dtype=type)[:, None]

cam = 0  # Select a specific camera

# Preallocate the output array depending on the selected camera
if cam == -1:
    X = np.zeros([mapFun.nCam, numPoints, 2], dtype=type, order='C')
else:
    X = np.zeros([numPoints, 2], dtype=type, order='C')

# Compute mapping using preallocated output
X1 = mapFun.worldToImg(points, 1, X)

# Verify whether X1 shares memory with X
print(f'id(X)={id(X)} id(X1)={id(X1)} same Id={id(X)==id(X1)}')

# The output array can be reused as input for successive calls
X2 = mapFun.worldToImg(points, 1, X1)
print(f'id(X2)={id(X2)} id(X1)={id(X1)} same Id={id(X2)==id(X1)}')

# If only one point needs to be mapped, use worldToImgPoint
# Input: a list with 3D world coordinates
# Output: a point in the image reference system
# Note: in this case cam must not be -1
point = mapFun.worldToImgPoint([0,0,0], cam)
print(f'{point.x} {point.y}')


# Example of inverse mapping (from image coordinates to world coordinates)
for Type in [np.float64, np.float32]:
    print(f'Type={Type} ******************************************')

    for cam in [-1, 0]:
        print(f'------------  cam={cam}  z ')

        # Generate world points
        points = np.array([[1,2,3]] * numPoints, dtype=Type, order='C') + \
                 np.array(np.arange(numPoints) * 0.001, dtype=Type)[:, None]

        # Forward mapping: world -> image
        X1 = mapFun.worldToImg(points, cam, None)

        # Inverse mapping: image -> world (using known z)
        z = np.array(points[:,2].T, order='C')
        x = mapFun.imgToWorld(X1, cam, z)

        # Compare reconstructed points with original ones
        print(x - points)

        # Copy points and add noise to x,y components
        xx = np.copy(points)
        maxNoise = 1
        xx[:, 0:2] += np.random.default_rng().random([xx.shape[0], 2]) * maxNoise

        # If all cameras are used, replicate data accordingly
        if cam == -1:
            xx = np.tile(xx, [2, 1, 1])

        # Inverse mapping with noisy input
        x = mapFun.imgToWorld(X1, cam, xx)

        print(f'cam={cam} Type={Type} x,y,z')
        print(x - points)