import matplotlib.pyplot as plt
import numpy as np
import cv2


def f(x, y):
    return np.exp(-((x**2) + y**2) / 3)


x_vals = np.linspace(-5, 5, 100)
y_vals = np.linspace(-5, 5, 100)


X, Y = np.meshgrid(x_vals, y_vals)

Z = f(X - 2, Y) + f(X + 2, Y)
Z = (Z > 0.4).astype(np.float32)
Z *= np.random.rand(*Z.shape)

for i, x in enumerate(x_vals):
    for j, y in enumerate(y_vals):
        color = f"{hex(int(Z[j, i] * 255))[2:]}"
        if color == "0":
            color = "#003"
        else:
            color = "#" + 3 * color
        plt.fill([x, x + 0.1, x + 0.1, x], [y, y, y + 0.1, y + 0.1], color=color)

nb_cams = 32
cam_poses = np.array(
    [[6 * np.cos(theta), 6 * np.sin(theta)] for theta in np.linspace(0, 2 * np.pi, nb_cams, endpoint=False)]
)
cam_rots = np.linspace(np.pi, 3 * np.pi, nb_cams, endpoint=False)
cam2world_projs = np.array(
    [
        [[np.cos(theta), -np.sin(theta), cam_pose[0]], [np.sin(theta), np.cos(theta), cam_pose[1]], [0, 0, 1]]
        for theta, cam_pose in zip(cam_rots, cam_poses)
    ]
)

for i in range(nb_cams):
    plt.plot(cam_poses[i][0], cam_poses[i][1], "ro")
    plt.text(cam_poses[i][0], cam_poses[i][1], str(i))
    x = np.array([[0, 0, 1], [0.5, -0.2, 1], [0.5, 0.2, 1], [0, 0, 1]]).T
    x = cam2world_projs[i] @ x
    plt.plot(x[0, :], x[1, :], "r-")


plt.xlim(-7, 7)
plt.ylim(-7, 7)
plt.axis("equal")
plt.savefig("data/peanut/peanut.png", dpi=300, bbox_inches="tight", pad_inches=0, transparent=True)
plt.close()

# draw 1d image of the scene for each camera
for i in range(nb_cams):

    # sort pixels by distance to camera
    cam_pose = cam_poses[i]
    pixels_dist = np.linalg.norm(np.array([X.flatten(), Y.flatten()]).T - cam_pose, axis=1)
    pixels_sort = np.argsort(pixels_dist)[::-1]

    x0 = -1
    x1 = 1

    plt.figure(f"img{i}")
    plt.fill([x0, x1, x1, x0], [0, 0, 0.2, 0.2], color="#000")
    plt.figure(f"mask{i}")
    plt.fill([x0, x1, x1, x0], [0, 0, 0.2, 0.2], color="#000")

    for j in pixels_sort:
        x, y = X.flatten()[j], Y.flatten()[j]
        color = f"{hex(int(Z.flatten()[j] * 255))[2:]}"
        if color == "0":
            continue
        color = "#" + 3 * color

        px = np.array([[x, y, 1], [x + 0.1, y, 1], [x + 0.1, y + 0.1, 1], [x, y + 0.1, 1]]).T
        px = np.linalg.inv(cam2world_projs[i]) @ px
        px /= px[0, :]

        x0 = px[1, :].min()
        x1 = px[1, :].max()

        plt.figure(f"img{i}")
        plt.fill([x0, x1, x1, x0], [0, 0, 0.2, 0.2], color=color)
        plt.figure(f"mask{i}")
        plt.fill([x0, x1, x1, x0], [0, 0, 0.2, 0.2], color="#fff")

    plt.figure(f"img{i}")
    plt.xlim(-1, 1)
    plt.ylim(0, 0.2)
    plt.axis("off")
    plt.savefig(f"data/peanut/images/Image{i:04}.png", dpi=300, bbox_inches="tight", pad_inches=0, transparent=True)
    plt.close()

    plt.figure(f"mask{i}")
    plt.xlim(-1, 1)
    plt.ylim(0, 0.2)
    plt.axis("off")
    plt.savefig(f"data/peanut/masks/Image{i:04}.png", dpi=300, bbox_inches="tight", pad_inches=0, transparent=True)
    plt.close()