/*******************************************************************************
* Copyright 2015-2021 Intel Corporation.
*
* This software and the related documents are Intel copyrighted  materials,  and
* your use of  them is  governed by the  express license  under which  they were
* provided to you (License).  Unless the License provides otherwise, you may not
* use, modify, copy, publish, distribute,  disclose or transmit this software or
* the related documents without Intel's prior written permission.
*
* This software and the related documents  are provided as  is,  with no express
* or implied  warranties,  other  than those  that are  expressly stated  in the
* License.
*******************************************************************************/

// A simple example of performing Optical Flow algorithm 
// using Intel(R) Integrated Primitives (Intel(R) IPP) functions:
//     ippiPyramidGetSize
//     ippiPyramidInit
//     ippiPyramidLayerDownGetSize_8u_C1R
//     ippiPyramidLayerDownInit_8u_C1R
//     ippiPyramidLayerDown_8u_C1R
//     ippiOpticalFlowPyrLKGetSize
//     ippiOpticalFlowPyrLKInit_8u_C1R


#include <stdio.h>
#include "ipp.h"

#define WIDTH  640  /* frames width  */
#define HEIGHT 480  /* frames height */

/* Next two defines are created to simplify code reading and understanding */
#define EXIT_MAIN exitLine:                                  /* Label for Exit */
#define check_sts(st) if((st) != ippStsNoErr) goto exitLine; /* Go to Exit if Intel(R) IPP function returned status different from ippStsNoErr */

/* Results of ippMalloc() are not validated because Intel(R) IPP functions perform bad arguments check and will return an appropriate status  */
void drawRect_8u(Ipp8u* pSrc, int srcStep, int x, int y, int w, int h)
{
    int i, j;
    for (j = y; j < y+h; j++) {
        for (i = x; i < x+w; i++) {
            pSrc[srcStep*j+i] = 255;
        }
    }
}

int main(void)
{
    IppStatus status = ippStsNoErr;
    Ipp8u* pPrevFrame = NULL, *pNextFrame = NULL;  /* Pointers to previous/next frames */
    int prevStep = 0, nextStep = 0;         /* Steps, in bytes, through the previous/next frames */
    IppiSize roiSize = { WIDTH, HEIGHT };   /* Size of previous/next ROI in pixels */
    int            numLevel = 5;            /* Pyramid level number */
    float          rate = 2.0f;             /* Pyramid rate  */
    Ipp16s pKernel[5] = {1,1,1,1,1};        /* Pyramid kernels */
    int            kerSize = 5;             /* Pyramid kernel size  */
    IppiPoint_32f *prevPt = NULL;           /* Coordinates on previous frame */
    IppiPoint_32f *nextPt = NULL;           /* Hint to coordinates on next frame */
    Ipp8s         *pStatus= NULL;           /* Array of differences between pPrev and pNext points */
    Ipp32f        *pError = NULL;           /* Array of result indicator */
    int            numFeat = 5;           /* Point number */
    int            winSize = 41;            /* Search window size */
    int            numIter = 5;             /* Iteration number */
    float          threshold = 0.0001f;

    Ipp8u* pBuf1 = NULL, *pPBuf2 = NULL, *pPBuf1 = NULL;   /* Pointer to the work buffers */

    Ipp8u* pBuf2    = NULL;
    Ipp8u* pStBuf1  = NULL, *pStBuf2 = NULL; /* Pointers to the buffer to initialize state structure for pyramids */
    Ipp8u* pOFStBuf = NULL;

    pPrevFrame = ippiMalloc_8u_C1(roiSize.width, roiSize.height, &prevStep);
    pNextFrame = ippiMalloc_8u_C1(roiSize.width, roiSize.height, &nextStep);
    prevPt = (IppiPoint_32f*)ippsMalloc_8u(numFeat * 2 * sizeof(IppiPoint_32f));
    nextPt = (IppiPoint_32f*)ippsMalloc_8u(numFeat * 2 * sizeof(IppiPoint_32f));
    pStatus = (Ipp8s*) ippsMalloc_8u(numFeat);
    pError = (Ipp32f*)ippsMalloc_8u(numFeat * sizeof(Ipp32f));

    check_sts(status = ippiSet_8u_C1R(0, pPrevFrame, prevStep, roiSize));
    check_sts(status = ippiSet_8u_C1R(0, pNextFrame, nextStep, roiSize));

    int x = 3;
    int y = 3;
    int w = 5;
    int h = 5;
    int shiftx = 3;
    int shifty = 4;

    /*imitate shift of rectangle*/
    drawRect_8u(pPrevFrame, prevStep, x, y, w, h);
    drawRect_8u(pNextFrame, nextStep, x+shiftx, y+shifty, w, h);

    check_sts( status = ippsSet_8u(0, (Ipp8u*)prevPt, numFeat * 2 * sizeof(IppiPoint_32f)) );
    check_sts( status = ippsSet_8u(0, (Ipp8u*)nextPt, numFeat * 2 * sizeof(IppiPoint_32f)) );
    
    prevPt[0].x = 0  ; prevPt[0].y = 0;
    prevPt[1].x = x  ; prevPt[1].y = y;
    prevPt[2].x = x+w; prevPt[2].y = y;
    prevPt[3].x = x  ; prevPt[3].y = y+h;
    prevPt[4].x = x+w; prevPt[4].y = y+h;

    {
        IppiPyramid *pPyr1 = NULL, *pPyr2 = NULL;   /* Pointer to the pointer to the pyramid structures */
        IppiOptFlowPyrLK *pOF = NULL;               /* Optical flow structure */
        int   PyrSize, BufSize, StateSize;          /* Size of pyramid structure, pyramid external work buffer and state structure */

        /* Compute the size of the structure for pyramids and the size of the required work buffer (in bytes) */
        check_sts( status = ippiPyramidGetSize(&PyrSize, &BufSize, numLevel, roiSize, rate) )

        pBuf1  = ippsMalloc_8u(BufSize);
        pPBuf1 = ippsMalloc_8u(PyrSize);

        /* Initialize structure for pyramids, calculates ROI for layers */
        check_sts( status = ippiPyramidInit(&pPyr1, numLevel, roiSize, rate, pPBuf1, pBuf1) )
        ippsFree(pBuf1); pBuf1 = NULL;

        /* Compute the size of the structure for pyramids and the size of the required work buffer (in bytes) */
        check_sts( status = ippiPyramidGetSize(&PyrSize, &BufSize, numLevel, roiSize, rate) )

        pBuf1  = ippsMalloc_8u(BufSize);
        pPBuf2 = ippsMalloc_8u(PyrSize);
        /* Initialize structure for pyramids, calculates ROI for layers */
        check_sts( status = ippiPyramidInit(&pPyr2, numLevel, roiSize, rate, pPBuf2, pBuf1) )
        ippsFree(pBuf1); pBuf1 = NULL;

        {
            IppiPyramidDownState_8u_C1R **pState1 = (IppiPyramidDownState_8u_C1R**)&(pPyr1->pState);/* Pyramid state structure */
            IppiPyramidDownState_8u_C1R **pState2 = (IppiPyramidDownState_8u_C1R**)&(pPyr2->pState);/* Pyramid state structure */
            Ipp8u   **pImg1 = pPyr1->pImage; /* Pointer to the source image */
            Ipp8u   **pImg2 = pPyr2->pImage; /* Pointer to the source image */
            int     *pStep1 = pPyr1->pStep, *pStep2 = pPyr2->pStep;
            IppiSize *pRoi1 = pPyr1->pRoi, *pRoi2 = pPyr2->pRoi;
            IppHintAlgorithm hint = ippAlgHintFast;
            int i, level = pPyr1->level;
            int pStateSize1, pBufSize1, pStateSize2, pBufSize2;

            /* Calculate the size of structure for creating a lower(an upper) pyramid layer and the size of the temporary buffer */
            check_sts( status = ippiPyramidLayerDownGetSize_8u_C1R(roiSize, rate, kerSize, &pStateSize1, &pBufSize1) )

            pBuf1 = ippsMalloc_8u(pBufSize1);
            pStBuf1 = ippsMalloc_8u(pStateSize1);

            /* Initialize the structure for creating a lower(an upper) pyramid layer */
            check_sts( status = ippiPyramidLayerDownInit_8u_C1R((IppiPyramidDownState_8u_C1R**)pState1, roiSize, rate, pKernel, kerSize, IPPI_INTER_LINEAR, pStBuf1, pBuf1) )
            ippsFree(pBuf1); pBuf1 = NULL;

            /* Calculate the size of structure for creating a lower(an upper) pyramid layer and the size of the temporary  buffer */
            check_sts( status = ippiPyramidLayerDownGetSize_8u_C1R(roiSize, rate, kerSize, &pStateSize2, &pBufSize2) )

            pBuf2 = ippsMalloc_8u(pBufSize2);
            pStBuf2 = ippsMalloc_8u(pStateSize2);

            /* Initialize the structure for creating a lower(an upper) pyramid layer */
            check_sts( status = ippiPyramidLayerDownInit_8u_C1R((IppiPyramidDownState_8u_C1R**)pState2, roiSize, rate, pKernel, kerSize, IPPI_INTER_LINEAR, pStBuf2, pBuf2) )
            ippsFree(pBuf2); pBuf2 = NULL;

            pImg1[0] = (Ipp8u*)pPrevFrame;
            pImg2[0] = (Ipp8u*)pNextFrame;
            pStep1[0] = prevStep;
            pStep2[0] = nextStep;
            pRoi1[0] = pRoi2[0] = roiSize;
            for (i = 1; i <= level; i++)
            {
                pPyr1->pImage[i] = ippiMalloc_8u_C1(pRoi1[i].width, pRoi1[i].height,
                    pStep1 + i);
                pPyr2->pImage[i] = ippiMalloc_8u_C1(pRoi2[i].width, pRoi2[i].height,
                    pStep2 + i);
                /* Perform downsampling / upsampling of the image with 5x5 Gaussian kernel*/
                check_sts( status = ippiPyramidLayerDown_8u_C1R(pImg1[i - 1], pStep1[i - 1], pRoi1[i - 1],
                    pImg1[i], pStep1[i], pRoi1[i], *pState1) )
                check_sts( status = ippiPyramidLayerDown_8u_C1R(pImg2[i - 1], pStep2[i - 1], pRoi2[i - 1],
                    pImg2[i], pStep2[i], pRoi2[i], *pState2) )
            }

            check_sts( status = ippiOpticalFlowPyrLKGetSize(winSize, roiSize, ipp8u, (IppHintAlgorithm)((int)hint), &StateSize) )

            pOFStBuf = (Ipp8u*)ippsMalloc_8u(StateSize);

            /* Initialize a structure for pyramidal L-K algorithm */
            check_sts( status = ippiOpticalFlowPyrLKInit_8u_C1R((IppiOptFlowPyrLK_8u_C1R**)&pOF, roiSize, winSize, (IppHintAlgorithm)((int)hint), pOFStBuf) )

            /* Indicate an error condition if maxLev or threshold has negative value, or maxIter has zero or negative value */
            check_sts( status = ippiOpticalFlowPyrLK_8u_C1R(pPyr1, pPyr2, prevPt, nextPt, pStatus, pError,
                numFeat, winSize, numLevel, numIter, threshold, pOF) )

            for (i = level; i > 0; i--)
            {
                if (pImg2[i]) ippiFree(pImg2[i]);
                if (pImg1[i]) ippiFree(pImg1[i]);
            }
        }
    }
    printf("shiftx=%d, ", shiftx);
    printf("shifty=%d\n", shifty);
    int n;
    for (n = 0; n < numFeat; n++) {
        printf("%f, %f -> %f, %f\n", prevPt[n].x, prevPt[n].y, nextPt[n].x, nextPt[n].y);
    }
EXIT_MAIN
    ippsFree(pOFStBuf);
    ippsFree(pBuf1);
    ippsFree(pBuf2);
    ippsFree(pPBuf1);
    ippsFree(pPBuf2);
    ippsFree(pStBuf1);
    ippsFree(pStBuf2);
    ippiFree(pPrevFrame);
    ippiFree(pNextFrame);
    ippsFree(prevPt);
    ippsFree(nextPt);
    ippsFree(pStatus);
    ippsFree(pError);
    printf("Exit status %d (%s)\n", (int)status, ippGetStatusString(status));
    return (int)status;
}