/*
 * Copyright (c) 2017-2018 Arm Limited.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#include "helpers.h"
#include "types.h"

/*
 *The criteria for lost tracking is that the spatial gradient matrix has:
 * - Determinant less than DETERMINANT_THR
 * - or minimum eigenvalue is smaller then EIGENVALUE_THR
 *
 * The thresholds for the determinant and the minimum eigenvalue is
 * defined by the OpenVX spec
 *
 * Note: Also lost tracking happens when the point tracked coordinate is outside
 * the image coordinates
 *
 * https://www.khronos.org/registry/vx/specs/1.0/html/d0/d0c/group__group__vision__function__opticalflowpyrlk.html
 */

/* Internal Lucas-Kanade Keypoint struct */
typedef struct InternalKeypoint
{
    float x;               /**< The x coordinate. */
    float y;               /**< The y coordinate. */
    float tracking_status; /**< A zero indicates a lost point. Initialized to 1 by corner detectors. */
    float dummy;           /**< Dummy member for alignment. */
} InternalKeypoint;

/** Threshold for the determinant. Used for lost tracking criteria */
#define DETERMINANT_THR 1.0e-07f

/** Thresholds for minimum eigenvalue. Used for lost tracking criteria */
#define EIGENVALUE_THR 1.0e-04f

/** Constants used for Lucas-Kanade Algorithm */
#define W_BITS (14)
#define FLT_SCALE (1.0f / (float)(1 << 20))
#define D0 ((float)(1 << W_BITS))
#define D1 (1.0f / (float)(1 << (W_BITS - 5)))

/** Initializes the internal new points array when the level of pyramid is NOT equal to max.
 *
 * @param[in,out] old_points_internal An array of internal key points that are defined at the old_images high resolution pyramid.
 * @param[in,out] new_points_internal An array of internal key points that are defined at the new_images high resolution pyramid.
 * @param[in]     scale               Scale factor to apply for the new_point coordinates.
 */
__kernel void init_level(
    __global float4 *old_points_internal,
    __global float4 *new_points_internal,
    const float      scale)
{
    int idx = get_global_id(0);

    // Get old and new keypoints
    float4 old_point = old_points_internal[idx];
    float4 new_point = new_points_internal[idx];

    // Scale accordingly with the pyramid_scale
    old_point.xy *= (float2)(2.0f);
    new_point.xy *= (float2)(2.0f);

    old_points_internal[idx] = old_point;
    new_points_internal[idx] = new_point;
}

/** Initializes the internal new points array when the level of pyramid is equal to max.
 *
 * @param[in]     old_points          An array of key points that are defined at the old_images high resolution pyramid.
 * @param[in,out] old_points_internal An array of internal key points that are defined at the old_images high resolution pyramid.
 * @param[out]    new_points_internal An array of internal key points that are defined at the new_images high resolution pyramid.
 * @param[in]     scale               Scale factor to apply for the new_point coordinates.
 */
__kernel void init_level_max(
    __global Keypoint *old_points,
    __global InternalKeypoint *old_points_internal,
    __global InternalKeypoint *new_points_internal,
    const float                scale)
{
    int idx = get_global_id(0);

    Keypoint old_point = old_points[idx];

    // Get old keypoint to track
    InternalKeypoint old_point_internal;
    old_point_internal.x               = old_point.x * scale;
    old_point_internal.y               = old_point.y * scale;
    old_point_internal.tracking_status = 1.f;

    // Store internal keypoints
    old_points_internal[idx] = old_point_internal;
    new_points_internal[idx] = old_point_internal;
}

/** Initializes the new_points array when the level of pyramid is equal to max and if use_initial_estimate = 1.
 *
 * @param[in]     old_points           An array of key points that are defined at the old_images high resolution pyramid.
 * @param[in]     new_points_estimates An array of estimate key points that are defined at the old_images high resolution pyramid.
 * @param[in,out] old_points_internal  An array of internal key points that are defined at the old_images high resolution pyramid.
 * @param[out]    new_points_internal  An array of internal key points that are defined at the new_images high resolution pyramid.
 * @param[in]     scale                Scale factor to apply for the new_point coordinates.
 */
__kernel void init_level_max_initial_estimate(
    __global Keypoint *old_points,
    __global Keypoint *new_points_estimates,
    __global InternalKeypoint *old_points_internal,
    __global InternalKeypoint *new_points_internal,
    const float                scale)
{
    int idx = get_global_id(0);

    Keypoint         old_point          = old_points[idx];
    Keypoint         new_point_estimate = new_points_estimates[idx];
    InternalKeypoint old_point_internal;
    InternalKeypoint new_point_internal;

    // Get old keypoint to track
    old_point_internal.x               = old_point.x * scale;
    old_point_internal.y               = old_point.y * scale;
    old_point_internal.tracking_status = 1.f;

    // Get new keypoint to track
    new_point_internal.x               = new_point_estimate.x * scale;
    new_point_internal.y               = new_point_estimate.y * scale;
    new_point_internal.tracking_status = new_point_estimate.tracking_status;

    // Store internal keypoints
    old_points_internal[idx] = old_point_internal;
    new_points_internal[idx] = new_point_internal;
}

/** Truncates the coordinates stored in new_points array
 *
 * @param[in]  new_points_internal An array of estimate key points that are defined at the new_images high resolution pyramid.
 * @param[out] new_points          An array of internal key points that are defined at the new_images high resolution pyramid.
 */
__kernel void finalize(
    __global InternalKeypoint *new_points_internal,
    __global Keypoint *new_points)
{
    int idx = get_global_id(0);

    // Load internal keypoint
    InternalKeypoint new_point_internal = new_points_internal[idx];

    // Calculate output point
    Keypoint new_point;
    new_point.x               = round(new_point_internal.x);
    new_point.y               = round(new_point_internal.y);
    new_point.strength        = 0.f;
    new_point.scale           = 0.f;
    new_point.orientation     = 0.f;
    new_point.tracking_status = new_point_internal.tracking_status;
    new_point.error           = 0.f;

    // Store new point
    new_points[idx] = new_point;
}

/** Computes A11, A12, A22, min_eig, ival, ixval and iyval at level 0th of the pyramid. These values will be used in step 1.
 *
 * @param[in]      old_image_ptr                               Pointer to the input old image. Supported data types: U8
 * @param[in]      old_image_stride_x                          Stride of the input old image in X dimension (in bytes)
 * @param[in]      old_image_step_x                            old_image_stride_x * number of elements along X processed per workitem(in bytes)
 * @param[in]      old_image_stride_y                          Stride of the input old image in Y dimension (in bytes)
 * @param[in]      old_image_step_y                            old_image_stride_y * number of elements along Y processed per workitem(in bytes)
 * @param[in]      old_image_offset_first_element_in_bytes     The offset of the first element in the input old image
 * @param[in]      old_scharr_gx_ptr                           Pointer to the input scharr x image. Supported data types: S16
 * @param[in]      old_scharr_gx_stride_x                      Stride of the input scharr x image in X dimension (in bytes)
 * @param[in]      old_scharr_gx_step_x                        old_scharr_gx_stride_x * number of elements along X processed per workitem(in bytes)
 * @param[in]      old_scharr_gx_stride_y                      Stride of the input scharr x image in Y dimension (in bytes)
 * @param[in]      old_scharr_gx_step_y                        old_scharr_gx_stride_y * number of elements along Y processed per workitem(in bytes)
 * @param[in]      old_scharr_gx_offset_first_element_in_bytes The offset of the first element in the input scharr x image
 * @param[in]      old_scharr_gy_ptr                           Pointer to the input scharr y image. Supported data types: S16
 * @param[in]      old_scharr_gy_stride_x                      Stride of the input scharr y image in X dimension (in bytes)
 * @param[in]      old_scharr_gy_step_x                        old_scharr_gy_stride_x * number of elements along X processed per workitem(in bytes)
 * @param[in]      old_scharr_gy_stride_y                      Stride of the input scharr y image in Y dimension (in bytes)
 * @param[in]      old_scharr_gy_step_y                        old_scharr_gy_stride_y * number of elements along Y processed per workitem(in bytes)
 * @param[in]      old_scharr_gy_offset_first_element_in_bytes The offset of the first element in the input scharr y image
 * @param[in]      old_points                                  An array of key points. Those key points are defined at the old_images high resolution pyramid
 * @param[in, out] new_points                                  An output array of key points. Those key points are defined at the new_images high resolution pyramid
 * @param[out]     coeff                                       It stores | A11 | A12 | A22 | min_eig | for each keypoint
 * @param[out]     iold_val                                    It stores | ival | ixval | iyval | dummy | for each point in the window centered on old_keypoint
 * @param[in]      window_dimension                            The size of the window on which to perform the algorithm
 * @param[in]      window_dimension_pow2                       The squared size of the window on which to perform the algorithm
 * @param[in]      half_window                                 The half size of the window on which to perform the algorithm
 * @param[in]      border_limits                               It stores the right border limit (width - window_dimension - 1, height - window_dimension - 1,)
 * @param[in]      eig_const                                   1.0f / (float)(2.0f * window_dimension * window_dimension)
 * @param[in]      level0                                      It is set to 1 if level 0 of the pyramid
 */
void __kernel lktracker_stage0(
    IMAGE_DECLARATION(old_image),
    IMAGE_DECLARATION(old_scharr_gx),
    IMAGE_DECLARATION(old_scharr_gy),
    __global float4 *old_points,
    __global float4 *new_points,
    __global float4 *coeff,
    __global short4 *iold_val,
    const int        window_dimension,
    const int        window_dimension_pow2,
    const int        half_window,
    const float3     border_limits,
    const float      eig_const,
    const int        level0)
{
    int idx = get_global_id(0);

    Image old_image     = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_image);
    Image old_scharr_gx = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_scharr_gx);
    Image old_scharr_gy = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_scharr_gy);

    // Get old keypoint
    float2 old_keypoint = old_points[idx].xy - (float2)half_window;

    // Get the floor value
    float2 iold_keypoint = floor(old_keypoint);

    // Check if using the window dimension we can go out of boundary in the following for loops. If so, invalidate the tracked point
    if(any(iold_keypoint < border_limits.zz) || any(iold_keypoint >= border_limits.xy))
    {
        if(level0 == 1)
        {
            // Invalidate tracked point as we are at level 0
            new_points[idx].s2 = 0.0f;
        }

        // Not valid coordinate. It sets min_eig to 0.0f
        coeff[idx].s3 = 0.0f;

        return;
    }

    // Compute weight for the bilinear interpolation
    float2 ab = old_keypoint - iold_keypoint;

    // Weight used for Bilinear-Interpolation on Scharr images
    // w_scharr.s0 = (1.0f - ab.x) * (1.0f - ab.y)
    // w_scharr.s1 = ab.x * (1.0f - ab.y)
    // w_scharr.s2 = (1.0f - ab.x) * ab.y
    // w_scharr.s3 = ab.x * ab.y

    float4 w_scharr;
    w_scharr.s3  = ab.x * ab.y;
    w_scharr.s0  = w_scharr.s3 + 1.0f - ab.x - ab.y;
    w_scharr.s12 = ab - (float2)w_scharr.s3;

    // Weight used for Bilinear-Interpolation on Old and New images
    // w.s0 = round(w_scharr.s0 * D0)
    // w.s1 = round(w_scharr.s1 * D0)
    // w.s2 = round(w_scharr.s2 * D0)
    // w.s3 = w.s3 = D0 - w.s0 - w.s1 - w.s2

    float4 w;
    w    = round(w_scharr * (float4)D0);
    w.s3 = D0 - w.s0 - w.s1 - w.s2; // Added for matching VX implementation

    // G.s0 = A11, G.s1 = A12, G.s2 = A22, G.s3 = min_eig
    int4 iG = (int4)0;

    // Window offset
    int window_offset = idx * window_dimension_pow2;

    // Compute Spatial Gradient Matrix G
    for(ushort ky = 0; ky < window_dimension; ++ky)
    {
        int offset_y = iold_keypoint.y + ky;
        for(ushort kx = 0; kx < window_dimension; ++kx)
        {
            int    offset_x = iold_keypoint.x + kx;
            float4 px;

            // Load values from old_image for computing the bilinear interpolation
            px = convert_float4((uchar4)(vload2(0, offset(&old_image, offset_x, offset_y)),
                                         vload2(0, offset(&old_image, offset_x, offset_y + 1))));

            // old_i.s0 = ival, old_i.s1 = ixval, old_i.s2 = iyval, old_i.s3 = dummy
            float4 old_i;

            // Compute bilinear interpolation (with D1 scale factor) for ival
            old_i.s0 = dot(px, w) * D1;

            // Load values from old_scharr_gx for computing the bilinear interpolation
            px = convert_float4((short4)(vload2(0, (__global short *)offset(&old_scharr_gx, offset_x, offset_y)),
                                         vload2(0, (__global short *)offset(&old_scharr_gx, offset_x, offset_y + 1))));

            // Compute bilinear interpolation for ixval
            old_i.s1 = dot(px, w_scharr);

            // Load values from old_scharr_gy for computing the bilinear interpolation
            px = convert_float4((short4)(vload2(0, (__global short *)offset(&old_scharr_gy, offset_x, offset_y)),
                                         vload2(0, (__global short *)offset(&old_scharr_gy, offset_x, offset_y + 1))));

            // Compute bilinear interpolation for iyval
            old_i.s2 = dot(px, w_scharr);

            // Rounding (it could be omitted. Used just for matching the VX implementation)
            int4 iold = convert_int4(round(old_i));

            // Accumulate values in the Spatial Gradient Matrix
            iG.s0 += (int)(iold.s1 * iold.s1);
            iG.s1 += (int)(iold.s1 * iold.s2);
            iG.s2 += (int)(iold.s2 * iold.s2);

            // Store ival, ixval and iyval
            iold_val[window_offset + kx] = convert_short4(iold);
        }
        window_offset += window_dimension;
    }

    // Scale iA11, iA12 and iA22
    float4 G = convert_float4(iG) * (float4)FLT_SCALE;

    // Compute minimum eigen value
    G.s3 = (float)(G.s2 + G.s0 - sqrt(pown(G.s0 - G.s2, 2) + 4.0f * G.s1 * G.s1)) * eig_const;

    // Store A11. A11, A22 and min_eig
    coeff[idx] = G;
}

/** Computes the motion vector for a given keypoint
 *
 * @param[in]      new_image_ptr                           Pointer to the input new image. Supported data types: U8
 * @param[in]      new_image_stride_x                      Stride of the input new image in X dimension (in bytes)
 * @param[in]      new_image_step_x                        new_image_stride_x * number of elements along X processed per workitem(in bytes)
 * @param[in]      new_image_stride_y                      Stride of the input new image in Y dimension (in bytes)
 * @param[in]      new_image_step_y                        new_image_stride_y * number of elements along Y processed per workitem(in bytes)
 * @param[in]      new_image_offset_first_element_in_bytes The offset of the first element in the input new image
 * @param[in, out] new_points                              An output array of key points. Those key points are defined at the new_images high resolution pyramid
 * @param[in]      coeff                                   The | A11 | A12 | A22 | min_eig | for each keypoint
 * @param[in]      iold_val                                The | ival | ixval | iyval | dummy | for each point in the window centered on old_keypoint
 * @param[in]      window_dimension                        The size of the window on which to perform the algorithm
 * @param[in]      window_dimension_pow2                   The squared size of the window on which to perform the algorithm
 * @param[in]      half_window                             The half size of the window on which to perform the algorithm
 * @param[in]      num_iterations                          The maximum number of iterations
 * @param[in]      epsilon                                 The value for terminating the algorithm.
 * @param[in]      border_limits                           It stores the right border limit (width - window_dimension - 1, height - window_dimension - 1,)
 * @param[in]      eig_const                               1.0f / (float)(2.0f * window_dimension * window_dimension)
 * @param[in]      level0                                  It is set to 1 if level of pyramid = 0
 * @param[in]      term_epsilon                            It is set to 1 if termination = TERM_CRITERIA_EPSILON
 */
void __kernel lktracker_stage1(
    IMAGE_DECLARATION(new_image),
    __global float4 *new_points,
    __global float4 *coeff,
    __global short4 *iold_val,
    const int        window_dimension,
    const int        window_dimension_pow2,
    const int        half_window,
    const int        num_iterations,
    const float      epsilon,
    const float3     border_limits,
    const float      eig_const,
    const int        level0,
    const int        term_epsilon)
{
    int   idx       = get_global_id(0);
    Image new_image = CONVERT_TO_IMAGE_STRUCT_NO_STEP(new_image);

    // G.s0 = A11, G.s1 = A12, G.s2 = A22, G.s3 = min_eig
    float4 G = coeff[idx];

    // Determinant
    float D = G.s0 * G.s2 - G.s1 * G.s1;

    // Check if it is a good point to track
    if(G.s3 < EIGENVALUE_THR || D < DETERMINANT_THR)
    {
        if(level0 == 1)
        {
            // Invalidate tracked point as we are at level 0
            new_points[idx].s2 = 0;
        }

        return;
    }

    // Compute inverse
    //D = native_recip(D);
    D = 1.0 / D;

    // Get new keypoint
    float2 new_keypoint = new_points[idx].xy - (float)half_window;

    // Get new point
    float2 out_new_point = new_points[idx].xy;

    // Keep delta obtained in the previous iteration
    float2 prev_delta = (float2)0.0f;

    int j = 0;
    while(j < num_iterations)
    {
        // Get the floor value
        float2 inew_keypoint = floor(new_keypoint);

        // Check if using the window dimension we can go out of boundary in the following for loops. If so, invalidate the tracked point
        if(any(inew_keypoint < border_limits.zz) || any(inew_keypoint >= border_limits.xy))
        {
            if(level0 == 1)
            {
                // Invalidate tracked point as we are at level 0
                new_points[idx].s2 = 0.0f;
            }
            else
            {
                new_points[idx].xy = out_new_point;
            }

            return;
        }

        // Compute weight for the bilinear interpolation
        float2 ab = new_keypoint - inew_keypoint;

        // Weight used for Bilinear-Interpolation on Old and New images
        // w.s0 = round((1.0f - ab.x) * (1.0f - ab.y) * D0)
        // w.s1 = round(ab.x * (1.0f - ab.y) * D0)
        // w.s2 = round((1.0f - ab.x) * ab.y * D0)
        // w.s3 = D0 - w.s0 - w.s1 - w.s2

        float4 w;
        w.s3  = ab.x * ab.y;
        w.s0  = w.s3 + 1.0f - ab.x - ab.y;
        w.s12 = ab - (float2)w.s3;
        w     = round(w * (float4)D0);
        w.s3  = D0 - w.s0 - w.s1 - w.s2;

        // Mismatch vector
        int2 ib = 0;

        // Old val offset
        int old_val_offset = idx * window_dimension_pow2;

        for(int ky = 0; ky < window_dimension; ++ky)
        {
            for(int kx = 0; kx < window_dimension; ++kx)
            {
                // ival, ixval and iyval have been computed in the previous stage
                int4 old_ival = convert_int4(iold_val[old_val_offset]);

                // Load values from old_image for computing the bilinear interpolation
                float4 px = convert_float4((uchar4)(vload2(0, offset(&new_image, inew_keypoint.x + kx, inew_keypoint.y + ky)),
                                                    vload2(0, offset(&new_image, inew_keypoint.x + kx, inew_keypoint.y + ky + 1))));

                // Compute bilinear interpolation on new image
                int jval = (int)round(dot(px, w) * D1);

                // Compute luminance difference
                int diff = (int)(jval - old_ival.s0);

                // Accumulate values in mismatch vector
                ib += (diff * old_ival.s12);

                // Update old val offset
                old_val_offset++;
            }
        }

        float2 b = convert_float2(ib) * (float2)FLT_SCALE;

        // Optical Flow
        float2 delta;

        delta.x = (float)((G.s1 * b.y - G.s2 * b.x) * D);
        delta.y = (float)((G.s1 * b.x - G.s0 * b.y) * D);

        // Update new point coordinate
        new_keypoint += delta;

        out_new_point = new_keypoint + (float2)half_window;

        if(term_epsilon == 1)
        {
            float mag2 = dot(delta, delta);

            if(mag2 <= epsilon)
            {
                new_points[idx].xy = out_new_point;

                return;
            }
        }

        // Check convergence analyzing the previous delta
        if(j > 0 && all(fabs(delta + prev_delta) < (float2)0.01f))
        {
            out_new_point -= delta * (float2)0.5f;

            new_points[idx].xy = out_new_point;

            return;
        }

        // Update previous delta
        prev_delta = delta;

        j++;
    }

    new_points[idx].xy = out_new_point;
}