import cv2
import numpy as np
import os
import sys


def get_eyes_nose_open_face(csv_path):

    return nose, (left_eye_x, left_eye_y), (right_eyes_x, right_eyes_y)


def rotate_point(origin, point, angle):
    ox, oy = origin
    px, py = point

    qx = ox + np.cos(angle) * (px - ox) - np.sin(angle) * (py - oy)
    qy = oy + np.sin(angle) * (px - ox) + np.cos(angle) * (py - oy)
    return qx, qy


def is_between(point1, point2, point3, extra_point):
    c1 = (point2[0] - point1[0]) * (extra_point[1] - point1[1]) - (point2[1] - point1[1]) * (extra_point[0] - point1[0])
    c2 = (point3[0] - point2[0]) * (extra_point[1] - point2[1]) - (point3[1] - point2[1]) * (extra_point[0] - point2[0])
    c3 = (point1[0] - point3[0]) * (extra_point[1] - point3[1]) - (point1[1] - point3[1]) * (extra_point[0] - point3[0])
    if (c1 < 0 and c2 < 0 and c3 < 0) or (c1 > 0 and c2 > 0 and c3 > 0):
        return True
    else:
        return False


def distance(a, b):
    return np.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)


def cosine_formula(length_line1, length_line2, length_line3):
    cos_a = -(length_line3 ** 2 - length_line2 ** 2 - length_line1 ** 2) / (2 * length_line2 * length_line1)
    return cos_a

def shape_to_normal(shape):
    shape_normal = []
    for i in range(0, 5):
        shape_normal.append((i, (shape.part(i).x, shape.part(i).y)))
    return shape_normal


def rotate_opencv(img, nose_center, angle):
    M = cv2.getRotationMatrix2D(nose_center, angle, 1)
    rotated = cv2.warpAffine(img, M, (img.shape[1], img.shape[0]), flags=cv2.INTER_CUBIC)
    return rotated


def align(img, landmarks):

    nose, left_eye, right_eye = landmarks

    center_of_forehead = ((left_eye[0] + right_eye[0]) // 2, (left_eye[1] + right_eye[1]) // 2)
    center_pred = (int((x + w) / 2), int((y + y) / 2))
    length_line1 = distance(center_of_forehead, nose)
    length_line2 = distance(center_pred, nose)
    length_line3 = distance(center_pred, center_of_forehead)
    cos_a = cosine_formula(length_line1, length_line2, length_line3)
    angle = np.arccos(cos_a)
    rotated_point = rotate_point(nose, center_of_forehead, angle)
    rotated_point = (int(rotated_point[0]), int(rotated_point[1]))
    if is_between(nose, center_of_forehead, center_pred, rotated_point):
        angle = np.degrees(-angle)
    else:
        angle = np.degrees(angle)

    img = rotate_opencv(img, nose, angle)

    return img

def align_sequences(frames_path, landmarks):
    file_list = glob.glob('{}/*.png'.format(frames_path))
    file_list.sort()
    data = []

    dir_aligned = os.path.join(frames_path,'aligned')

    if not os.path.isdir(dir_aligned):
        os.makedirs(dir_aligned)

    print("Reading frames")
    current_num_samples = 0
    for f in range(0, file_list.__len__()):
        try:
            data.append(cv2.imread(file_list[f]))

    for i in range(len(landmarks)):
        try:
            aligned_img = align(data[i],landmarks[i])
            output_aligned_path = 'frame_%6d'.format(i)
            cv2.imwrite(output_aligned_path,aligned_img)
        except:
            print("Error Aligning frame")


if __name__ == '__main__':
    landmarks = get_eyes_nose_open_face("csvpath")
    align_sequences('folder containing frames from ffmpeg',landmarks)