/*******************************************************************************
* Copyright 2015-2020 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  */

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 = 200;           /* 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 = ippsSet_8u(0, (Ipp8u*)prevPt, numFeat * 2 * sizeof(IppiPoint_32f)) );
    check_sts( status = ippsSet_8u(0, (Ipp8u*)nextPt, numFeat * 2 * sizeof(IppiPoint_32f)) );
    {
        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]);
            }
        }
    }

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;
}