/* ----------------------------------------------------------------------
 * Project:      CMSIS DSP Library
 * Title:        arm_svm_rbf_predict_f32.c
 * Description:  SVM Radial Basis Function Classifier
 *
 * $Date:        23 April 2021
 * $Revision:    V1.9.0
 *
 * Target Processor: Cortex-M and Cortex-A cores
 * -------------------------------------------------------------------- */
/*
 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the License); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "dsp/svm_functions.h"
#include <limits.h>
#include <math.h>


/**
 * @addtogroup rbfsvm
 * @{
 */


/**
 * @brief SVM rbf prediction
 * @param[in]    S         Pointer to an instance of the rbf SVM structure.
 * @param[in]    in        Pointer to input vector
 * @param[out]   pResult   decision value
 * @return none.
 *
 */

#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)

#include "arm_helium_utils.h"
#include "arm_vec_math.h"

void arm_svm_rbf_predict_f32(
    const arm_svm_rbf_instance_f32 *S,
    const float32_t * in,
    int32_t * pResult)
{
        /* inlined Matrix x Vector function interleaved with dot prod */
    uint32_t        numRows = S->nbOfSupportVectors;
    uint32_t        numCols = S->vectorDimension;
    const float32_t *pSupport = S->supportVectors;
    const float32_t *pSrcA = pSupport;
    const float32_t *pInA0;
    const float32_t *pInA1;
    uint32_t         row;
    uint32_t         blkCnt;     /* loop counters */
    const float32_t *pDualCoef = S->dualCoefficients;
    float32_t       sum = S->intercept;
    f32x4_t         vSum = vdupq_n_f32(0);

    row = numRows;

    /*
     * compute 4 rows in parrallel
     */
    while (row >= 4) {
        const float32_t *pInA2, *pInA3;
        float32_t const *pSrcA0Vec, *pSrcA1Vec, *pSrcA2Vec, *pSrcA3Vec, *pInVec;
        f32x4_t         vecIn, acc0, acc1, acc2, acc3;
        float32_t const *pSrcVecPtr = in;

        /*
         * Initialize the pointers to 4 consecutive MatrixA rows
         */
        pInA0 = pSrcA;
        pInA1 = pInA0 + numCols;
        pInA2 = pInA1 + numCols;
        pInA3 = pInA2 + numCols;
        /*
         * Initialize the vector pointer
         */
        pInVec = pSrcVecPtr;
        /*
         * reset accumulators
         */
        acc0 = vdupq_n_f32(0.0f);
        acc1 = vdupq_n_f32(0.0f);
        acc2 = vdupq_n_f32(0.0f);
        acc3 = vdupq_n_f32(0.0f);

        pSrcA0Vec = pInA0;
        pSrcA1Vec = pInA1;
        pSrcA2Vec = pInA2;
        pSrcA3Vec = pInA3;

        blkCnt = numCols >> 2;
        while (blkCnt > 0U) {
            f32x4_t         vecA;
            f32x4_t         vecDif;

            vecIn = vld1q(pInVec);
            pInVec += 4;
            vecA = vld1q(pSrcA0Vec);
            pSrcA0Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);
            vecA = vld1q(pSrcA1Vec);
            pSrcA1Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc1 = vfmaq(acc1, vecDif, vecDif);
            vecA = vld1q(pSrcA2Vec);
            pSrcA2Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc2 = vfmaq(acc2, vecDif, vecDif);
            vecA = vld1q(pSrcA3Vec);
            pSrcA3Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc3 = vfmaq(acc3, vecDif, vecDif);

            blkCnt--;
        }
        /*
         * tail
         * (will be merged thru tail predication)
         */
        blkCnt = numCols & 3;
        if (blkCnt > 0U) {
            mve_pred16_t    p0 = vctp32q(blkCnt);
            f32x4_t         vecA;
            f32x4_t         vecDif;

            vecIn = vldrwq_z_f32(pInVec, p0);
            vecA = vldrwq_z_f32(pSrcA0Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);
            vecA = vldrwq_z_f32(pSrcA1Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc1 = vfmaq(acc1, vecDif, vecDif);
            vecA = vldrwq_z_f32(pSrcA2Vec, p0);;
            vecDif = vsubq(vecIn, vecA);
            acc2 = vfmaq(acc2, vecDif, vecDif);
            vecA = vldrwq_z_f32(pSrcA3Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc3 = vfmaq(acc3, vecDif, vecDif);
        }
        /*
         * Sum the partial parts
         */

        //sum += *pDualCoef++ * expf(-S->gamma * vecReduceF32Mve(acc0));
        f32x4_t         vtmp = vuninitializedq_f32();
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc0), vtmp, 0);
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc1), vtmp, 1);
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc2), vtmp, 2);
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc3), vtmp, 3);

        vSum =
            vfmaq_f32(vSum, vld1q(pDualCoef),
                      vexpq_f32(vmulq_n_f32(vtmp, -S->gamma)));
        pDualCoef += 4;
        pSrcA += numCols * 4;
        /*
         * Decrement the row loop counter
         */
        row -= 4;
    }

    /*
     * compute 2 rows in parrallel
     */
    if (row >= 2) {
        float32_t const *pSrcA0Vec, *pSrcA1Vec, *pInVec;
        f32x4_t         vecIn, acc0, acc1;
        float32_t const *pSrcVecPtr = in;

        /*
         * Initialize the pointers to 2 consecutive MatrixA rows
         */
        pInA0 = pSrcA;
        pInA1 = pInA0 + numCols;
        /*
         * Initialize the vector pointer
         */
        pInVec = pSrcVecPtr;
        /*
         * reset accumulators
         */
        acc0 = vdupq_n_f32(0.0f);
        acc1 = vdupq_n_f32(0.0f);
        pSrcA0Vec = pInA0;
        pSrcA1Vec = pInA1;

        blkCnt = numCols >> 2;
        while (blkCnt > 0U) {
            f32x4_t         vecA;
            f32x4_t         vecDif;

            vecIn = vld1q(pInVec);
            pInVec += 4;
            vecA = vld1q(pSrcA0Vec);
            pSrcA0Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);;
            vecA = vld1q(pSrcA1Vec);
            pSrcA1Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc1 = vfmaq(acc1, vecDif, vecDif);

            blkCnt--;
        }
        /*
         * tail
         * (will be merged thru tail predication)
         */
        blkCnt = numCols & 3;
        if (blkCnt > 0U) {
            mve_pred16_t    p0 = vctp32q(blkCnt);
            f32x4_t         vecA, vecDif;

            vecIn = vldrwq_z_f32(pInVec, p0);
            vecA = vldrwq_z_f32(pSrcA0Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);
            vecA = vldrwq_z_f32(pSrcA1Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc1 = vfmaq(acc1, vecDif, vecDif);
        }
        /*
         * Sum the partial parts
         */
        f32x4_t         vtmp = vuninitializedq_f32();
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc0), vtmp, 0);
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc1), vtmp, 1);

        vSum =
            vfmaq_m_f32(vSum, vld1q(pDualCoef),
                        vexpq_f32(vmulq_n_f32(vtmp, -S->gamma)), vctp32q(2));
        pDualCoef += 2;

        pSrcA += numCols * 2;
        row -= 2;
    }

    if (row >= 1) {
        f32x4_t         vecIn, acc0;
        float32_t const *pSrcA0Vec, *pInVec;
        float32_t const *pSrcVecPtr = in;
        /*
         * Initialize the pointers to last MatrixA row
         */
        pInA0 = pSrcA;
        /*
         * Initialize the vector pointer
         */
        pInVec = pSrcVecPtr;
        /*
         * reset accumulators
         */
        acc0 = vdupq_n_f32(0.0f);

        pSrcA0Vec = pInA0;

        blkCnt = numCols >> 2;
        while (blkCnt > 0U) {
            f32x4_t         vecA, vecDif;

            vecIn = vld1q(pInVec);
            pInVec += 4;
            vecA = vld1q(pSrcA0Vec);
            pSrcA0Vec += 4;
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);

            blkCnt--;
        }
        /*
         * tail
         * (will be merged thru tail predication)
         */
        blkCnt = numCols & 3;
        if (blkCnt > 0U) {
            mve_pred16_t    p0 = vctp32q(blkCnt);
            f32x4_t         vecA, vecDif;

            vecIn = vldrwq_z_f32(pInVec, p0);
            vecA = vldrwq_z_f32(pSrcA0Vec, p0);
            vecDif = vsubq(vecIn, vecA);
            acc0 = vfmaq(acc0, vecDif, vecDif);
        }
        /*
         * Sum the partial parts
         */
        f32x4_t         vtmp = vuninitializedq_f32();
        vtmp = vsetq_lane(vecAddAcrossF32Mve(acc0), vtmp, 0);

        vSum =
            vfmaq_m_f32(vSum, vld1q(pDualCoef),
                        vexpq_f32(vmulq_n_f32(vtmp, -S->gamma)), vctp32q(1));

    }


    sum += vecAddAcrossF32Mve(vSum);
    *pResult = S->classes[STEP(sum)];
}


#else
#if defined(ARM_MATH_NEON)

#include "NEMath.h"

void arm_svm_rbf_predict_f32(
    const arm_svm_rbf_instance_f32 *S,
    const float32_t * in,
    int32_t * pResult)
{
    float32_t sum = S->intercept;
   
    float32_t dot;
    float32x4_t dotV; 

    float32x4_t accuma,accumb,accumc,accumd,accum;
    float32x2_t accum2;
    float32x4_t temp;
    float32x4_t vec1;

    float32x4_t vec2,vec2a,vec2b,vec2c,vec2d;

    uint32_t blkCnt;   
    uint32_t vectorBlkCnt;   

    const float32_t *pIn = in;

    const float32_t *pSupport = S->supportVectors;

    const float32_t *pSupporta = S->supportVectors;
    const float32_t *pSupportb;
    const float32_t *pSupportc;
    const float32_t *pSupportd;

    pSupportb = pSupporta + S->vectorDimension;
    pSupportc = pSupportb + S->vectorDimension;
    pSupportd = pSupportc + S->vectorDimension;

    const float32_t *pDualCoefs = S->dualCoefficients;


    vectorBlkCnt = S->nbOfSupportVectors >> 2;
    while (vectorBlkCnt > 0U)
    {
        accuma = vdupq_n_f32(0);
        accumb = vdupq_n_f32(0);
        accumc = vdupq_n_f32(0);
        accumd = vdupq_n_f32(0);

        pIn = in;

        blkCnt = S->vectorDimension >> 2;
        while (blkCnt > 0U)
        {
        
            vec1 = vld1q_f32(pIn);
            vec2a = vld1q_f32(pSupporta);
            vec2b = vld1q_f32(pSupportb);
            vec2c = vld1q_f32(pSupportc);
            vec2d = vld1q_f32(pSupportd);

            pIn += 4;
            pSupporta += 4;
            pSupportb += 4;
            pSupportc += 4;
            pSupportd += 4;

            temp = vsubq_f32(vec1, vec2a);
            accuma = vmlaq_f32(accuma, temp, temp);

            temp = vsubq_f32(vec1, vec2b);
            accumb = vmlaq_f32(accumb, temp, temp);

            temp = vsubq_f32(vec1, vec2c);
            accumc = vmlaq_f32(accumc, temp, temp);

            temp = vsubq_f32(vec1, vec2d);
            accumd = vmlaq_f32(accumd, temp, temp);

            blkCnt -- ;
        }
        accum2 = vpadd_f32(vget_low_f32(accuma),vget_high_f32(accuma));
        dotV = vsetq_lane_f32(vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1),dotV,0);

        accum2 = vpadd_f32(vget_low_f32(accumb),vget_high_f32(accumb));
        dotV = vsetq_lane_f32(vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1),dotV,1);

        accum2 = vpadd_f32(vget_low_f32(accumc),vget_high_f32(accumc));
        dotV = vsetq_lane_f32(vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1),dotV,2);

        accum2 = vpadd_f32(vget_low_f32(accumd),vget_high_f32(accumd));
        dotV = vsetq_lane_f32(vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1),dotV,3);


        blkCnt = S->vectorDimension & 3;
        while (blkCnt > 0U)
        {
            dotV = vsetq_lane_f32(vgetq_lane_f32(dotV,0) + SQ(*pIn - *pSupporta), dotV,0);
            dotV = vsetq_lane_f32(vgetq_lane_f32(dotV,1) + SQ(*pIn - *pSupportb), dotV,1);
            dotV = vsetq_lane_f32(vgetq_lane_f32(dotV,2) + SQ(*pIn - *pSupportc), dotV,2);
            dotV = vsetq_lane_f32(vgetq_lane_f32(dotV,3) + SQ(*pIn - *pSupportd), dotV,3);

            pSupporta++;
            pSupportb++;
            pSupportc++;
            pSupportd++;

            pIn++;

            blkCnt -- ;
        }

        vec1 = vld1q_f32(pDualCoefs);
        pDualCoefs += 4; 

        // To vectorize later
        dotV = vmulq_n_f32(dotV, -S->gamma);
        dotV = vexpq_f32(dotV);

        accum = vmulq_f32(vec1,dotV);
        accum2 = vpadd_f32(vget_low_f32(accum),vget_high_f32(accum));
        sum += vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1);

        pSupporta += 3*S->vectorDimension;
        pSupportb += 3*S->vectorDimension;
        pSupportc += 3*S->vectorDimension;
        pSupportd += 3*S->vectorDimension;

        vectorBlkCnt -- ;
    }

    pSupport = pSupporta;
    vectorBlkCnt = S->nbOfSupportVectors & 3;

    while (vectorBlkCnt > 0U)
    {
        accum = vdupq_n_f32(0);
        dot = 0.0f;
        pIn = in;

        blkCnt = S->vectorDimension >> 2;
        while (blkCnt > 0U)
        {
        
            vec1 = vld1q_f32(pIn);
            vec2 = vld1q_f32(pSupport);
            pIn += 4;
            pSupport += 4;

            temp = vsubq_f32(vec1,vec2);
            accum = vmlaq_f32(accum, temp,temp);

            blkCnt -- ;
        }
        accum2 = vpadd_f32(vget_low_f32(accum),vget_high_f32(accum));
        dot = vget_lane_f32(accum2, 0) + vget_lane_f32(accum2, 1);


        blkCnt = S->vectorDimension & 3;
        while (blkCnt > 0U)
        {

            dot = dot + SQ(*pIn - *pSupport);
            pIn++;
            pSupport++;

            blkCnt -- ;
        }

        sum += *pDualCoefs++ * expf(-S->gamma * dot);
        vectorBlkCnt -- ;
    }

    *pResult=S->classes[STEP(sum)];
}
#else
void arm_svm_rbf_predict_f32(
    const arm_svm_rbf_instance_f32 *S,
    const float32_t * in,
    int32_t * pResult)
{
    float32_t sum=S->intercept;
    float32_t dot=0;
    uint32_t i,j;
    const float32_t *pSupport = S->supportVectors;

    for(i=0; i < S->nbOfSupportVectors; i++)
    {
        dot=0;
        for(j=0; j < S->vectorDimension; j++)
        {
            dot = dot + SQ(in[j] - *pSupport);
            pSupport++;
        }
        sum += S->dualCoefficients[i] * expf(-S->gamma * dot);
    }
    *pResult=S->classes[STEP(sum)];
}
#endif

#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */

/**
 * @} end of rbfsvm group
 */