import random
import typing
import argparse
import openmesh as om
import numpy as np
from numpy import sin, cos, pi
from tqdm import trange


def torus_param(uv, R, r):
    """
    Parametrization of a standard torus of revolution
    :param uv: 2 x n array with coordinates in the parametrization domain [0,2*Pi)^2
    :param R: radius of the latitudinal circular cross-section
    :param r: radius of the longitudinal circular cross-section
    :return: 3 x n array of mapped points
    """
    p1 = R * np.array([cos(uv[0]), sin(uv[0]), [0] * len(uv[0]) if isinstance(uv[0], typing.Iterable) else 0])
    p2 = r * np.array([(cos(uv[0])) * cos(uv[1]), (sin(uv[0])) * cos(uv[1]), sin(uv[1])])
    return p1 + p2


def ellipse_parametrization(t, eta, a, b):
    """
    Parametrization of a warped ellipse
    :param t: 1D array with points in the parameter domain
    :param eta: phase-shift of the ellipse controlling the warping and hence the rotation of the ellipse
    :param a: first semi-axis length
    :param b: second semi-axis length
    :return: 2 x n array of mapped points
    """
    return np.array([a * cos(eta / 2.) * cos(t - eta / 2.) - b * sin(eta / 2.) * sin(t - eta / 2.),
                     a * sin(eta / 2.) * cos(t - eta / 2.) + b * cos(eta / 2.) * sin(t - eta / 2.)])


def dt_ellipse_parametrization(t, eta, a, b):
    """
    Derivative of parametrization of a warped ellipse
    :param t: 1D array with points in the parameter domain
    :param eta: phase-shift of the ellipse controlling the warping and hence the rotation of the ellipse
    :param a: first semi-axis length
    :param b: second semi-axis length
    :return: 2 x n array of derivatives
    """
    return np.array([-a * cos(eta / 2.) * sin(t - eta / 2.) - b * sin(eta / 2.) * cos(t - eta / 2.),
                     -a * sin(eta / 2.) * sin(t - eta / 2.) + b * cos(eta / 2.) * cos(t - eta / 2.)])


def freaky_param(uv, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r):
    """
    Parametrization of a warped torus with elliptical cross-sections
    :param uv: 2 x n array with coordinates in the parametrization domain [0,2*Pi)^2
    :param R: radius of the latitudinal cross-section
    :param r: radius of the longitudinal cross-section
    :param a_R: first (relative) semi-axis length of the latitudinal cross-section
    :param b_R: second (relative) semi-axis length of the latitudinal cross-section
    :param a_r: first (relative) semi-axis length of the longitudinal cross-section
    :param b_r: first (relative) semi-axis length of the longitudinal cross-section
    :param phi_R: rotation/warping of the latitudinal cross-section
    :param phi_r: rotation/warping of the longitudinal cross-section
    :return: 3 x n array of mapped points
    """
    tau_R = ellipse_parametrization(uv[0], phi_R, a_R, b_R)
    tau_r = ellipse_parametrization(uv[1], phi_r, a_r, b_r)

    return R * np.array([tau_R[0], tau_R[1], [0] * len(uv[0]) if isinstance(uv[0], typing.Iterable) else 0]) + \
        r * np.array([cos(uv[0]) * tau_r[0], sin(uv[0]) * tau_r[0], tau_r[1]])


def du_freaky_param(uv, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r):
    """
    Derivative of a warped torus w.r.t. first parameter
    :param uv: 2 x n array with coordinates in the parametrization domain [0,2*Pi)^2
    :param R: radius of the latitudinal cross-section
    :param r: radius of the longitudinal cross-section
    :param a_R: first (relative) semi-axis length of the latitudinal cross-section
    :param b_R: second (relative) semi-axis length of the latitudinal cross-section
    :param a_r: first (relative) semi-axis length of the longitudinal cross-section
    :param b_r: first (relative) semi-axis length of the longitudinal cross-section
    :param phi_R: rotation/warping of the latitudinal cross-section
    :param phi_r: rotation/warping of the longitudinal cross-section
    :return: 3 x n array of derivatives
    """
    p1 = R * np.array([-a_R * cos(phi_R) * sin(uv[0] - phi_R) - b_R * sin(phi_R) * cos(uv[0] - phi_R),
                       -a_R * sin(phi_R) * sin(uv[0] - phi_R) + b_R * cos(phi_R) * cos(uv[0] - phi_R),
                       [0] * len(uv[0]) if isinstance(uv[0], typing.Iterable) else 0])
    p2 = r * np.array([-sin(uv[0]) * (a_r * cos(phi_r) * cos(uv[1] - phi_r) - b_r * sin(phi_r) * sin(uv[1] - phi_r)),
                       cos(uv[0]) * (a_r * cos(phi_r) * cos(uv[1] - phi_r) - b_r * sin(phi_r) * sin(uv[1] - phi_r)),
                       [0] * len(uv[0]) if isinstance(uv[0], typing.Iterable) else 0])
    return p1 + p2


def dv_freaky_param(uv, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r):
    """
    Derivative of a warped torus w.r.t. second parameter
    :param uv: 2 x n array with coordinates in the parametrization domain [0,2*Pi)^2
    :param R: radius of the latitudinal cross-section
    :param r: radius of the longitudinal cross-section
    :param a_R: first (relative) semi-axis length of the latitudinal cross-section
    :param b_R: second (relative) semi-axis length of the latitudinal cross-section
    :param a_r: first (relative) semi-axis length of the longitudinal cross-section
    :param b_r: first (relative) semi-axis length of the longitudinal cross-section
    :param phi_R: rotation/warping of the latitudinal cross-section
    :param phi_r: rotation/warping of the longitudinal cross-section
    :return: 3 x n array of derivatives
    """
    p2 = r * np.array([cos(uv[0]) * (-a_r * cos(phi_r) * sin(uv[1] - phi_r) - b_r * sin(phi_r) * cos(uv[1] - phi_r)),
                       sin(uv[0]) * (-a_r * cos(phi_r) * sin(uv[1] - phi_r) - b_r * sin(phi_r) * cos(uv[1] - phi_r)),
                       -a_r * sin(phi_r) * sin(uv[1] - phi_r) + b_r * cos(phi_r) * cos(uv[1] - phi_r)])
    return p2


def bump(r, eps):
    """
    Gaussian bump function
    :param r: array of distances
    :param eps: parameter of the Gaussian
    :return: array of bump fct values
    """
    return np.exp(-(eps * r) ** 2)


def normal_displacement(uv, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r, h, eps, uv_mid):
    """
    Displacement for a bump in normal direction on the warped torus
    :param uv: 2 x n array with coordinates in the parametrization domain [0,2*Pi)^2
    :param R: radius of the latitudinal cross-section
    :param r: radius of the longitudinal cross-section
    :param a_R: first (relative) semi-axis length of the latitudinal cross-section
    :param b_R: second (relative) semi-axis length of the latitudinal cross-section
    :param a_r: first (relative) semi-axis length of the longitudinal cross-section
    :param b_r: first (relative) semi-axis length of the longitudinal cross-section
    :param phi_R: rotation/warping of the latitudinal cross-section
    :param phi_r: rotation/warping of the longitudinal cross-section
    :param h: max. height of bump
    :param eps: size of bump (parameter of Gaussian)
    :param uv_mid: middle point of bump in the parametrization domain
    :return: n x 3 array of displacements
    """
    n = np.cross(du_freaky_param(uv_mid, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r),
                 dv_freaky_param(uv_mid, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r))
    n /= np.linalg.norm(n)

    dist = np.linalg.norm(freaky_param(uv, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r).T -
                          freaky_param(uv_mid, R, r, a_R, b_R, a_r, b_r, phi_R, phi_r), axis=-1)

    return h * bump(dist, 1 / eps).reshape(-1, 1) * n


if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Create a dataset of freaky torii',
                                     formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument('--N', type=int, help='Number of samples to create', default=1000)
    parser.add_argument('--R', type=float, help='radius of the latitudinal cross-section', default=0.375)
    parser.add_argument('--a_R', metavar="a_R", type=float,
                        help='first (relative) semi-axis length of the latitudinal cross-section', default=1.)
    parser.add_argument('--b_R', metavar="b_R", type=float,
                        help='second (relative) semi-axis length of the latitudinal cross-section', default=0.5)
    parser.add_argument('--r', metavar="r", type=float, help='radius of the longitudinal cross-section', default=0.125)
    parser.add_argument('--a_r', metavar="a_r", type=float,
                        help='first (relative) semi-axis length of the longitudinal cross-section', default=1.)
    parser.add_argument('--b_r', metavar="b_r", type=float,
                        help='second (relative) semi-axis length of the longitudinal cross-section', default=0.5)
    parser.add_argument('--height', metavar="h", type=float, help='max. height of bump', default=0.075)
    parser.add_argument('--eps', metavar="eps", type=float, help='size of bump (parameter of Gaussian)', default=0.05)
    parser.add_argument('--dir', type=str, help='output directory', default="data/")
    parser.add_argument('--torus-mesh', type=str,
                        help='triangulation of the torus, with parameters in texture coordinates',
                        default="data/raw_torus.ply")
    parser.add_argument('--export-meshes', action='store_true', help='also export dataset as meshes')

    args = parser.parse_args()


    def F(phi_R, phi_r, uv_mid):
        return lambda uv: freaky_param(uv, args.R, args.r, args.a_R, args.b_R, args.a_r, args.b_r, phi_R, phi_r).T + \
                          normal_displacement(uv, args.R, args.r, args.a_R, args.b_R, args.a_r, args.b_r, phi_R, phi_r,
                                              args.height, args.eps, uv_mid)


    mesh = om.read_trimesh(args.torus_mesh, vertex_tex_coord=True, vertex_normal=True)

    uv = mesh.vertex_texcoords2D()
    n = mesh.vertex_normals()
    xyz = mesh.points().copy()

    uv_mid = np.array([0, 0])

    Coordinates = []
    Coordinates_R = []
    Coordinates_r = []
    Coordinates_UV = []

    VertexPositions = []
    VertexPositions_R = []
    VertexPositions_r = []
    VertexPositions_UV = []

    # reference shape
    xyz = F(pi, pi, np.array([pi, pi]))(uv.T * 2 * pi)

    for i in range(uv.shape[0]):
        mesh.set_point(mesh.vertex_handle(i), xyz[i])

    mesh.update_normals()

    om.write_mesh(f'{args.dir}/reference.ply', mesh, vertex_tex_coord=True, vertex_normal=True)

    for j in trange(args.N):
        uv_mid[0] = random.uniform(0, 2 * pi)
        uv_mid[1] = random.uniform(0, 2 * pi)
        phi_R = random.uniform(0, 2 * pi)
        phi_r = random.uniform(0, 2 * pi)

        Coordinates.append([phi_R, phi_r, uv_mid[0], uv_mid[1]])
        Coordinates_R.append([phi_R])
        Coordinates_r.append([phi_r])
        Coordinates_UV.append([uv_mid[0], uv_mid[1]])

        # only R
        xyz = F(phi_R, pi, np.array([pi, pi]))(uv.T * 2 * pi)
        VertexPositions_R.append(xyz.T.flatten())
        if args.export_meshes:
            for i in range(uv.shape[0]):
                mesh.set_point(mesh.vertex_handle(i), xyz[i])

            mesh.update_normals()

            om.write_mesh(f'{args.dir}/rnd_R_{j}.ply', mesh, vertex_tex_coord=True, vertex_normal=True)

        # only r
        xyz = F(pi, phi_r, np.array([pi, pi]))(uv.T * 2 * pi)
        VertexPositions_r.append(xyz.T.flatten())

        if args.export_meshes:
            for i in range(uv.shape[0]):
                mesh.set_point(mesh.vertex_handle(i), xyz[i])

            mesh.update_normals()

            om.write_mesh(f'{args.dir}/rnd_r_{j}.ply', mesh, vertex_tex_coord=True, vertex_normal=True)

        # only bump
        xyz = F(pi, pi, uv_mid)(uv.T * 2 * pi)
        VertexPositions_UV.append(xyz.T.flatten())

        if args.export_meshes:
            for i in range(uv.shape[0]):
                mesh.set_point(mesh.vertex_handle(i), xyz[i])

            mesh.update_normals()

            om.write_mesh(f'{args.dir}/rnd_UV_{j}.ply', mesh, vertex_tex_coord=True, vertex_normal=True)

        # all
        xyz = F(phi_R, phi_r, uv_mid)(uv.T * 2 * pi)
        VertexPositions.append(xyz.T.flatten())

        if args.export_meshes:
            for i in range(uv.shape[0]):
                mesh.set_point(mesh.vertex_handle(i), xyz[i])

            mesh.update_normals()

            om.write_mesh(f'{args.dir}/rnd_{j}.ply', mesh, vertex_tex_coord=True, vertex_normal=True)

    np.save(f"{args.dir}/Coordinates_0.npy", np.array(Coordinates_R).T)
    np.save(f"{args.dir}/Coordinates_1.npy", np.array(Coordinates_r).T)
    np.save(f"{args.dir}/Coordinates_2.npy", np.array(Coordinates_UV).T)
    np.save(f"{args.dir}/Coordinates.npy", np.array(Coordinates).T)

    np.save(f"{args.dir}/VertexPositions_0.npy", np.array(VertexPositions_R).T)
    np.save(f"{args.dir}/VertexPositions_1.npy", np.array(VertexPositions_r).T)
    np.save(f"{args.dir}/VertexPositions_2.npy", np.array(VertexPositions_UV).T)
    np.save(f"{args.dir}/VertexPositions.npy", np.array(VertexPositions).T)
