#!/usr/bin/env python3

# test program for the "perspective" geometric transform element

import cv2
import numpy as np
from math import cos, sin, radians

import gi
gi.require_version('Gst', '1.0')
from gi.repository import Gst
Gst.init(None)

from gi.repository import GLib
from gi.repository import GObject


def calc_matrix():
    top_x_shift = 200

    width, height = 800, 480
    corners = np.array([(0, 0), (width, 0), (width, height), (0, height)], dtype=np.float32)
    target = np.array([(top_x_shift, 0), (width - top_x_shift, 0), (width, height), (0, height)], dtype=np.float32)

    mat = cv2.getPerspectiveTransform(corners, target)
    ret, inv_mat = cv2.invert(mat)

    return inv_mat.flatten()


def calc_rotation_matrix():
    width, height = 800, 480

    pivot = (width / 2, height / 2)
    angle = 10
    theta = radians(angle)

    # The dimensions of the bounding box of the rotated rectangle
    W = width * abs(cos(theta)) + height * abs(sin(theta))
    H = width * abs(sin(theta)) + height * abs(cos(theta))

    scale_factor = 1 / min(width / W, height / H)

    mat = cv2.getRotationMatrix2D(pivot, angle, scale_factor)
    mat = np.vstack([mat, [0, 0, 1]])

    return mat.flatten()


def main():
    pipeline = Gst.ElementFactory.make('pipeline', None)

    videosrc = Gst.ElementFactory.make('videotestsrc', None)
    pipeline.add(videosrc)

    # Test the perspective element
    perspective = Gst.ElementFactory.make("perspective", None)
    pipeline.add(perspective)

    print(perspective.get_property("matrix"))

    M = calc_rotation_matrix()
    M2 = GObject.ValueArray()
    for v in M:
        M2.append(float(v))
    perspective.set_property("matrix", M2)
    print(perspective.get_property("matrix"))

    # This should fail!
    perspective.set_property("matrix", [0])
    print(perspective.get_property("matrix"))

    videoconvert = Gst.ElementFactory.make("autovideoconvert", None)
    pipeline.add(videoconvert)

    videosink = Gst.ElementFactory.make("autovideosink", None)
    pipeline.add(videosink)

    caps = Gst.caps_from_string(
        "video/x-raw,format=\"AYUV\",width=800,height=480")

    videosrc.link_filtered(perspective, caps)
    perspective.link(videoconvert)
    videoconvert.link(videosink)

    pipeline.set_state(Gst.State.PLAYING)

    loop = GLib.MainLoop()
    loop.run()


if __name__ == '__main__':
    main()