import os
from os.path import join
from pathlib import Path
import math

import cv2 as cv
import numpy as np
import imutils

from app.utils import im_utils, logger_utils
from app.models.bbox import BBox
from app.settings import app_cfg as cfg
from app.settings import types



class FacePoseDLIB:


  dnn_size = (400, 400)
  pose_types = {'pitch': (0,0,255), 'roll': (255,0,0), 'yaw': (0,255,0)}

  def __init__(self):
    pass


  def pose(self, landmarks, dim):
    # computes pose using 6 / 68 points from dlib face landmarks
    # based on learnopencv.com and 
    # https://github.com/jerryhouuu/Face-Yaw-Roll-Pitch-from-Pose-Estimation-using-OpenCV/
    # NB: not as accurate as MTCNN, see @jerryhouuu for ideas
    
    pose_points_idx = (30, 8, 36, 45, 48, 54)
    axis = np.float32([[500,0,0], [0,500,0], [0,0,500]])
    
    # 3D model points.
    model_points = np.array([
        (0.0, 0.0, 0.0),             # Nose tip
        (0.0, -330.0, -65.0),        # Chin
        (-225.0, 170.0, -135.0),     # Left eye left corner
        (225.0, 170.0, -135.0),      # Right eye right corne
        (-150.0, -150.0, -125.0),    # Left Mouth corner
        (150.0, -150.0, -125.0)      # Right mouth corner
    ])
    
    # Assuming no lens distortion
    dist_coeffs = np.zeros((4,1))

    # find 6 pose points
    pose_points = []
    for j, idx in enumerate(pose_points_idx):
      pt = landmarks[idx]
      pose_points.append((pt[0], pt[1]))
    pose_points = np.array(pose_points, dtype='double')  # convert to double
    
    # create camera matrix
    focal_length = dim[0]
    center = (dim[0]/2, dim[1]/2)
    cam_mat = np.array(
        [[focal_length, 0, center[0]],
        [0, focal_length, center[1]],
        [0, 1, 1]], dtype = "double")
    
    # solve PnP for rotation and translation
    (success, rot_vec, tran_vec) = cv.solvePnP(model_points, pose_points, 
                                               cam_mat, dist_coeffs, 
                                               flags=cv.SOLVEPNP_ITERATIVE)

    result = {}

    # project points
    #if project_points:
    pts_im, jac = cv.projectPoints(axis, rot_vec, tran_vec, cam_mat, dist_coeffs)
    pts_model, jac2 = cv.projectPoints(model_points, rot_vec, tran_vec, cam_mat, dist_coeffs)
    #result['points_model'] = pts_model
    #result['points_image'] = pts_im
    result['points'] = {
      'pitch': pts_im[0],
      'roll': pts_im[2],
      'yaw': pts_im[1]
      }
    result['point_nose'] = tuple(landmarks[pose_points_idx[0]])

    rvec_matrix = cv.Rodrigues(rot_vec)[0]
    
    # convert to degrees
    proj_matrix = np.hstack((rvec_matrix, tran_vec))
    eulerAngles = cv.decomposeProjectionMatrix(proj_matrix)[6] 
    pitch, yaw, roll = [math.radians(x) for x in eulerAngles]
    pitch = math.degrees(math.asin(math.sin(pitch)))
    roll = -math.degrees(math.asin(math.sin(roll)))
    yaw = math.degrees(math.asin(math.sin(yaw)))
    result['pitch'] = pitch
    result['roll'] = roll
    result['yaw'] = yaw

    return result


  def draw_pose(self, im, pt_nose, image_pts):
    cv.line(im, pt_nose, tuple(image_pts['pitch'].ravel()), self.pose_types['pitch'], 3)
    cv.line(im, pt_nose, tuple(image_pts['yaw'].ravel()), self.pose_types['yaw'], 3)
    cv.line(im, pt_nose, tuple(image_pts['roll'].ravel()), self.pose_types['roll'], 3)


  def draw_degrees(self, im, pose_data, color=(0,255,0)):
    for i, pose_type in enumerate(self.pose_types.items()):
      k, clr = pose_type
      v = pose_data[k]
      t = '{}: {:.2f}'.format(k, v)
      origin = (10, 30 + (25 * i))
      cv.putText(im, t, origin, cv.FONT_HERSHEY_SIMPLEX, 0.5, clr, thickness=2, lineType=2)