#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
    two_dim_view

    Neil E Cotter
    06/28/2021
    Changed to funcname proj_to_2D.

    Neil E Cotter
    06/24/2021
    Changed filename from view_3dim_in_2dim.py and funcname from v32.

    Neil E Cotter
    12/30/2020
    minor modifications, changed func name

    Atul Ahirrao
    09/17/2019
    ported from MATLAB

    Fatemeh Koohestan Mahalian and Neil E Cotter
    05/23/2018
    original function in MATLAB
"""

import numpy as np
#from numpy.linalg import inv
import math


def proj_to_2D(view, projected_points) :
    """
    Image_plane_point_view_space_coordinate_mat  =
          Image3Din2D(view_point, focal_point, Projected_points)

    Translate pts in a 3-dimensional space to their projection on
    2-dimensional image plane that is normal to and centered on a line
    from a view point (where the viewer is located) to a focal point in
    the 3-D space (that will be projected onto the center point of the
    image plane).

    Note that the focal point merely refers to what will be the center
    point of the image.

    The bottom of the image plane is parallel to the x-y plane, which
    may be thought of as a floor in the 3-dimensional space.
    The z-axis points up from the x-y plane.

    Inputs: (all numpy arrays)
      view_point = [x,y,z] point where viewer's eye is located
      focal_point = [x,y,z] point the viewer is looking at in the 3-D
      space
      Projected_points = [[x1,y1,z1],[x2,y2,z2], ...] points to be
        projected to 2-Dim frame.  Values of points in rows.
    Outputs:
      Image_plane_point_view_space_coordinate_mat = [[x1,y1],[x2,y2],
      ...] projected points on 2-Dim image plane
    """

    # Storing view_point, focal_point, projected_points as numpy arrays.
    vp = np.asarray(view.view_point)
    #print("vp = ",vp)

    fp = np.asarray(view.focal_point)
    #print("fp = ",fp)
    
    pp = np.asarray(projected_points)
    
    # Make pp two-dimensional.
    if pp.ndim == 1:
        pp = np.array([pp])
    #print("pp = \n",pp)

    # Calculate vec (and its distance) from focal_point to view_point.
    v0_pt = np.subtract(vp, fp)
    #print("v0_pt = ",v0_pt)

    d = np.sqrt(np.sum(v0_pt ** 2, axis=0))
    #print("d = ",d)

    # Find number of points being projected.
    # number_points = len(pp)

    # Offset co-ord of Projected_points in 3-dim space by subtracting
    #  focal_point from each of them. This moves origin in 3-dim space
    #  to focal_point.
    #p_pts = pp - fp
    p_pts = np.subtract(pp,fp)
    #print("p_pts = \n",p_pts)

    # Calculate angles for the projection.
    # Angle between xy-plane and vec v0 from focal_point to view_point.
    phi = math.acos(v0_pt[2] / d)
    #print("phi = ",phi)

    # Distance on xy-plane from focal_point to view_point.
    # Value not used but may be computed as follows:
    # d_xy = d * math.sin(phi)
    #print("d_xy =",d_xy)

    # Angle of rotation of v0 in xy-plane.
    theta = math.atan2(v0_pt[1], v0_pt[0])
    #print("theta =",theta)

    v10_pt = np.array([-math.sin(theta), math.cos(theta), 0])
    #print("v10_pt = ",v10_pt)

    v20_pt = np.array([-math.cos(theta) * math.cos(phi),
            -math.sin(theta) * math.cos(phi), math.sin(phi)])
    #print("v20_pt = ",v20_pt)
    
    # v30 = [cos(theta)*sin(phi), sin(theta)*sin(phi), cos(phi)];
    v30_pt = np.array([v0_pt / d])
    #print("v30_pt = ",v30_pt)

    # Create a matrix for changing coord from 3-dim coord (with origin
    # at focal_point) to coord of image plane (and distance in front of
    #  image plane).
    Change_basis_mat = np.vstack((v10_pt, v20_pt, v30_pt))
    #print("Change_basis_mat = \n",Change_basis_mat)

    #Inv_change_basis_mat = inv(Change_basis_mat.transpose())
    Inv_change_basis_mat = np.linalg.inv(Change_basis_mat.transpose())
    #print("Inv_change_basis_mat = \n",Inv_change_basis_mat)

    # Create array of zeros to receive 2-dim image points.
#    tupp = p_pts.shape  # num_cols = 3 since points are in 3-dim space.
    num_rows, num_cols = p_pts.shape
    #print("tupp = ",tupp)

#    num_rows = tupp[0]
#    num_cols = tupp[1]

    Image_plane_pt_view_space_coord_mat = np.zeros((num_rows, num_cols))

    # Step through the image points, one at a time.
    for pt_num in range(0, num_rows):
        # Extract next point from array of points to transform.
        point3D = p_pts[pt_num, 0:num_cols]
        #print("point3D = \n",point3D)

        # Reshape from 1D to 2D vector which makes row or column sense.
        point3D = point3D.reshape((1, 3))
        #print("point3D = \n",point3D)

        # Find where 3-dim pt appears on image plane in original 3-dim
        #  coords.
        Image_plane_pt_Euclidean_coord_mat = (d * (v30_pt *
          point3D.dot(v30_pt.transpose()) - point3D)
          / (point3D.dot(v30_pt.transpose()) - d))
        #print("Image_plane_pt_Euclidean_coord_mat = \n")
        #print(Image_plane_pt_Euclidean_coord_mat)

        # Reshape from 1D to 2D vector which makes row or column sense
        Image_plane_pt_Euclidean_coord_mat = \
            Image_plane_pt_Euclidean_coord_mat.reshape((1, 3))
        #print("Image_plane_pt_Euclidean_coordinate_mat = \n")
        #print(Image_plane_pt_Euclidean_coord_mat)

        # Change coord to image plane coords. (Thus z value always = 0.)
        B = Inv_change_basis_mat.dot( 
          Image_plane_pt_Euclidean_coord_mat.transpose())
        #print("B = \n",B)

        # Take transpose of previous result to get pts in rows.  
        Image_plane_pt_view_space_coord_mat[pt_num, 0:num_cols] = \
            B.transpose()

    #print("Image_plane_pt_view_space_coord_mat = \n")
    #print(Image_plane_pt_view_space_coord_mat)

    return Image_plane_pt_view_space_coord_mat