/*
Copyright (c) 2017, Texas Instruments Incorporated - http://www.ti.com/
All rights reserved.

* Redistribution and use in source and binary forms, with or without 
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/

//
//
// Performance Audio Framework Series 3 -- Audio Stream Processing Definitions
//
//
//

#include "fwkPort.h"

#include <xdc/std.h>

//#include <mem.h>
#include <xdc/runtime/Log.h>
#include <xdc/runtime/Memory.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/IHeap.h>
#include <ti/sysbios/heaps/HeapMem.h>

#include "asp0.h"
//#include <paf_alg.h>
//#include <pafhjt.h>
#include <acp.h>
#include "logp.h"

#include "pfp/pfp.h"
#include "pfp_app.h"        /* contains all PFP ID's */


#include "dbgBenchmark.h" // PCM high-sampling rate + SRC + CAR benchmarking
//
// Performing PCM high-sampling rate + SRC + CAR benchmarking
//
#ifdef _ENABLE_BENCHMARK_PCMHSR_SRC_CAR_
#define PROFILER
#endif

#ifdef PROFILER
#include <ti/sysbios/knl/Task.h>
#include "ti/sysbios/knl/Clock.h"
#include "xdc/runtime/Timestamp.h"
#include "ti/sysbios/utils/Load.h"

#ifndef MAX_NUM
#define MAX_NUM 28515
#define NUM_ASP_PROFILE  0  // Profile CAR; As we have PCM->SRC(0)->CAR(1)->PCE
#endif
UInt32 profile_array_dsp_cycles[MAX_NUM];
UInt32 profile_array_dsp_cpuload[MAX_NUM];
UInt32 profile_index = 0;      // Array index
UInt32 start_profiling = 0;    // Flag
UInt32 currentAspCount = 0;    // ASP number in chain starting from 0
UInt32 aspNumberToProfile = NUM_ASP_PROFILE; // ASP number in chain starting from 0
volatile UInt32 time32_1, time32_2;
#endif

#ifdef _TMS320C6X
#define ASP_NUM_PROFILE  ( 0 )              // Profile CAR; As we have DEC->CAR(0)->OAR(1)->BMDA(2)->ENC
UInt32 gAspProfileEnable = 0;               // ASP profile enable flag
//UInt32 gAspNumProfile = ASP_NUM_PROFILE;    // ASP number in chain starting from 0
Int32 gAspNumProfile = -1;                 // Number of ASP in chain to profile or -1 to indicate all ASPs
//UInt32 gAspCurCount = 0;                    // ASP number in chain starting from 0
Int32 gAspPfpId;                           // Number of ASP in chain to profile or -1 to indicate all ASPs
#endif // _TMS320C6X


//
// Audio Stream Processing Chain Function - Initialization
//
//   Name:      PAF_ASP_chainInit_
//   Purpose:   Initialize an ASP Chain by linking all elements of an
//              initialization array onto it. If this is the first audio
//              stream, perform base initialization of the ASP Chain, but 
//              otherwise do not.
//   From:      AST Parameter Chain Function -> chainInit
//   Uses:      AST Parameter Chain Function -> chainLink
//   States:    None.
//   Return:    ASP Chain.
//   Trace:     None.
//

PAF_ASP_Chain *
PAF_ASP_chainInit_ (
    PAF_ASP_Chain *this,
    const struct PAF_ASP_ChainFxns *fxns,
    HeapMem_Handle heap, //int heap,
    Uns stream,
    ACP_Handle acp,
    LOG_Obj *log,
    const PAF_ASP_LinkInit *pInit,
    PAF_ASP_Chain *from,
    IALG_MemRec *common,
    PAF_IALG_Config *pafConfig)
{
    if (stream) {
        this->fxns = fxns;
        this->head = NULL;
        this->stream = stream;
        this->acp = acp;
        this->log = log;
    }

    for ( ; pInit && pInit->thisCode.full; pInit++)
        if (! this->fxns->chainLink (this, heap, pInit, from, common, pafConfig))
            return NULL;

    return this;
}

//
// Audio Stream Processing Chain Function - Link
//
//   Name:      PAF_ASP_chainLink_
//   Purpose:   Link an element onto an ASP Chain:
//              (1) Allocate memory for the link, and
//              (2) For the first gear, create the data for the link, but
//              (3) For other than the first gear, simply link the data already
//                  created in the previous gear.
//   From:      AST Parameter Chain Function -> chainInit
//   Uses:      See code.
//   States:    None.
//   Return:    Success: ASP Chain.
//              Failure: Null.
//   Trace:     Message Log "trace" in Debug Project Configuration reports:
//              * Memory allocation errors.
//              * Data creation errors.
//              * Data linking errors.
//


PAF_ASP_Chain *
PAF_ASP_chainLink_ (
    PAF_ASP_Chain *this,
    HeapMem_Handle heap, //int heap,
    const PAF_ASP_LinkInit *pInit,
    PAF_ASP_Chain *from,
    IALG_MemRec *common,
    PAF_IALG_Config *pafConfig)
{
    PAF_ASP_Link *pLink;
    Error_Block    eb;

    if (! this || ! pInit)
        return this;

    // Initialize error block
    Error_init(&eb); 

    //if (! (pLink = MEM_alloc (heap, sizeof(PAF_ASP_Link), 4))) {
    if (!(pLink = Memory_alloc((IHeap_Handle)heap, sizeof(PAF_ASP_Link), 4, &eb))) {
        if (this->log)
            LOG_printf(this->log, "AS%d: ASP code 0x%08x alloc failure", 
                this->stream, pInit->thisCode.full);
        return NULL;
    }

    if (! from) {
        if (! this->fxns->linkData(pLink, pInit, this->stream, this->acp, 
            this->log,common, pafConfig)) {
            if (this->log)
                LOG_printf(this->log, "AS%d: ASP code 0x%08x link-init failure",
                    this->stream, pInit->thisCode.full);
            return NULL;
        }
    }
    else {
        if (! this->fxns->linkCopy(pLink, pInit, from->head)) {
            if (this->log)
                LOG_printf(this->log, "AS%d: ASP code 0x%08x link-copy failure",
                    this->stream, pInit->thisCode.full);
            return NULL;
        }
    }

    {
        PAF_ASP_Link **ppLink;
        for (ppLink=&this->head; *ppLink; ppLink=&(*ppLink)->next)
            if ((*ppLink)->code.full == pInit->linkCode.full)
                break;
        pLink->next = *ppLink;
        *ppLink = pLink;
    }

    return this;
}

//
// Audio Stream Processing Chain Function - Find
//
//   Name:      PAF_ASP_chainFind_
//   Purpose:   Find an element in an ASP Chain.
//   From:      AST Parameter Chain Function -> chainFind
//   Uses:      None.
//   States:    None.
//   Return:    Success (found): Pointer to element.
//              Failure (not found): Null.
//   Trace:     None.
//

PAF_ASP_Link *
PAF_ASP_chainFind_ (
    PAF_ASP_Chain *this,
    PAF_ASP_AlphaCode code)
{
    PAF_ASP_Link *that = this->head;

    for ( ; that; that = that->next) {
        if (that->code.full == code.full)
            break;
    }

    return that;
}

//
// Audio Stream Processing Chain Function - Reset
//
//   Name:      PAF_ASP_chainReset_
//   Purpose:   Reset an ASP Chain by invoking for each element:
//              (1) The algorithm activation function (NOT PERFORMED), and
//              (2) The algorithm reset function.
//   From:      AST Parameter Chain Function -> chainReset
//   Uses:      See code.
//   States:    None.
//   Return:    0 on success.
//              Error number reported by ASP Reset Function on failure.
//   Trace:     Message Log "trace" in Debug Project Configuration reports:
//              * ASP Reset Function errors.
//

Int
PAF_ASP_chainReset_ (PAF_ASP_Chain *this, PAF_AudioFrame *pAudioFrame)
{
    Int errno;

    PAF_ASP_Link *pLink;

    for (pLink=this->head; pLink; pLink=pLink->next) {
        ASP_Handle alg = pLink->alg;
#if 0
        /* ASP Algorithms are self-activating and not framework-activated */
        if (((ALG_Handle )alg)->fxns->algActivate)
            ((ALG_Handle )alg)->fxns->algActivate (((ALG_Handle )alg));
#endif
        if (alg->fxns->reset && (errno = alg->fxns->reset(alg, pAudioFrame))) {
            if (this->log) {
                LOG_printf(this->log, "AS%d: ASP code 0x%08x reset error",
                    this->stream, pLink->code.full);
                LOG_printf(this->log, "AS%d: errno = 0x%08x", 
                    this->stream, errno);
            }
            return errno;
        }
    }

    return 0;
}

//
// Audio Stream Processing Chain Function - Apply
//
//   Name:      PAF_ASP_chainApply_
//   Purpose:   Apply an ASP Chain by invoking for each:
//              (1) The algorithm apply function.
//   From:      AST Parameter Chain Function -> chainApply
//   Uses:      See code.
//   States:    None.
//   Return:    0 on success.
//              Error number reported by ASP Apply Function on failure.
//   Trace:     Message Log "trace" in Debug Project Configuration reports:
//              * ASP Apply Function errors.
//

Int
PAF_ASP_chainApply_ (PAF_ASP_Chain *this, PAF_AudioFrame *pAudioFrame)
{
    Int errno;
    int i = 0;
    PAF_ASP_Link *pLink;

    for (pLink=this->head; pLink; pLink=pLink->next)
    {
        ASP_Handle alg = pLink->alg;

        i++;
        if (this->log)
        {
            LOG_printf(&trace, "PAF_ASP_chainApply_: AS_%d. link %d.  alg: 0x%x.",
                       this->stream, i, alg);
        }
        Log_info3("PAF_ASP_chainApply_: AS_%d. link %d.  alg: 0x%x.", this->stream, i, (IArg)alg);
  
        if (alg->fxns->apply /*&& (errno = alg->fxns->apply(alg, pAudioFrame))*/)
        {
#ifdef PROFILER
            if (aspNumberToProfile == currentAspCount)  // only profile the selected ASP
            {
                time32_1 = Timestamp_get32();
            }
#endif

#ifdef _TMS320C6X
            if (gAspProfileEnable == 1)
            {
                if (gAspNumProfile == (i-1))
                {
                    // only profile the selected ASP
                    pfpBegin(PFP_ID_ASOT_ASP_0, Task_self());
                }
                else if ((gAspNumProfile == -1) && (i <= PFP_MAX_ASP))
                {
                    // profile current ASP
                    pfpBegin(PFP_ID_ASOT_ASP_0+(i-1), Task_self());
                }
            }
#endif            

            errno = alg->fxns->apply(alg, pAudioFrame);
            
#ifdef _TMS320C6X
            if (gAspProfileEnable == 1)
            {
                if (gAspNumProfile == (i-1))
                {
                    // only profile the selected ASP
                    pfpEnd(PFP_ID_ASOT_ASP_0, PFP_FINISH_MEAS);
                }
                else if ((gAspNumProfile == -1) && (i <= PFP_MAX_ASP))
                {
                    // profile current ASP
                    pfpEnd(PFP_ID_ASOT_ASP_0+(i-1), PFP_FINISH_MEAS);
                }
            }
#endif            

#ifdef PROFILER
            if (aspNumberToProfile == currentAspCount)  // only profile the selected ASP
            {
                time32_2 = Timestamp_get32();
                //Load_Stat stat;
                //Load_getTaskLoad (Task_getIdleTask(), &stat);
                //UInt32 intLoad = Load_calculateLoad (&stat);
                if (start_profiling == 1)
                {
                    if (profile_index < MAX_NUM)
                    {
                        profile_array_dsp_cycles[profile_index] = (time32_2 - time32_1);
                        //profile_array_dsp_cpuload[profile_index] = (100   - intLoad);
                        profile_array_dsp_cpuload[profile_index] =  Load_getCPULoad();
                        profile_index++;
                    }
                    else
                    {
                        //SW_BREAKPOINT;
                    }
                }

                //TRACE_TERSE1("profiler: Timestamp_get32() cycles: %u", (time32_2 - time32_1) * factor);
                //TRACE_TERSE2("profiler: CPU load: idle: %d other: %d", intLoad, (100 - intLoad));
            }
            // increase ASP count
            currentAspCount++;
#endif

            if (errno && this->log)
            {
                LOG_printf(this->log, "AS%d: ASP code 0x%08x apply error 0x%x.",
                    this->stream, pLink->code.full, errno);
                LOG_printf(&trace, "AS%d: ASP code 0x%08x apply error 0x%x.",
                    this->stream, pLink->code.full, errno);

#ifdef PROFILER
                // Reset ASP count for next frame
                currentAspCount = 0;
#endif

                return errno;
            }
        }
    }
    
#ifdef PROFILER
    // Reset ASP count for next frame
    currentAspCount = 0;
#endif

    return 0;
}

//
// Audio Stream Processing Chain Function - Final
//
//   Name:      PAF_ASP_chainFinal_
//   Purpose:   Finalize an ASP Chain by invoking for each:
//              (1) The algorithm final function (NOT PERFORMED), and
//              (2) The algorithm deactivation function.
//   From:      AST Parameter Chain Function -> chainFinal
//   Uses:      See code.
//   States:    None.
//   Return:    0 on success.
//              Error number reported by ASP Final Function on failure.
//   Trace:     Message Log "trace" in Debug Project Configuration reports:
//              * ASP Final Function errors.
//
//   Note:      In Audio Framework #2, PAF_ASP_FINAL is not defined and there
//              are no Final Functions supplied with ASP Algorithms.

Int
PAF_ASP_chainFinal_ (PAF_ASP_Chain *this, PAF_AudioFrame *pAudioFrame)
{
#ifdef PAF_ASP_FINAL
    Int errno;
#endif /* PAF_ASP_FINAL */

    PAF_ASP_Link *pLink;

    for (pLink=this->head; pLink; pLink=pLink->next) {
        ASP_Handle alg = pLink->alg;
#ifdef PAF_ASP_FINAL
        if (alg->fxns->final && (errno = alg->fxns->final(alg, pAudioFrame))) {
            if (this->log) {
                LOG_printf(this->log, "AS%d: ASP code 0x%08x final error",
                    this->stream, pLink->code.full);
                LOG_printf(this->log, "AS%d: errno = 0x%08x", 
                    this->stream, errno);
            }
            return errno;
        }
#endif /* PAF_ASP_FINAL */
#if 1
        /* ASP Algorithms are self-deactivating and framework-deactivated */
        if (((ALG_Handle )alg)->fxns->algDeactivate)
            ((ALG_Handle )alg)->fxns->algDeactivate (((ALG_Handle )alg));
#endif
    }

    return 0;
}

//
// Audio Stream Processing Chain Function - Link Data
//
//   Name:      PAF_ASP_linkData
//   Purpose:   Create the data for a chain link:
//              (1) Perform beta unit relocation,
//              (2) Instantiate an algorithm, and
//              (3) Attach the beta unit(s) of an algorithm to an ACP Algorithm
//                  instance.
//   From:      AST Parameter Chain Function -> linkData
//   Uses:      See code.
//   States:    None.
//   Return:    Null pointer on failure.
//              Pointer to the chain link on success.
//   Trace:     Message Log "trace" in Debug Project Configuration reports:
//              * Algorithm instance creation failure message.
//              * Beta Unit initialization success message.
//

PAF_ASP_Link *
PAF_ASP_linkData (
    PAF_ASP_Link *this,
    const PAF_ASP_LinkInit *pInit,
    Uns stream,
    ACP_Handle acp,
    LOG_Obj *log,
    IALG_MemRec *common,
    PAF_IALG_Config *pafConfig) 
{
    IALG_Status *pStatus;
    
    ALG_Handle alg;
    
    Int beta = pInit->thisCode.part.beta;
        
    if (log)
        LOG_printf(log, "AS%d: beta 0x%x initialization begins.", stream, beta);
    {
        ALG_Handle acpAlg = (ALG_Handle) acp;
        Int betaPrimeBase;
        Int betaPrimeOffset;
        acpAlg->fxns->algControl (acpAlg, ACP_GETBETAPRIMEBASE, 
            (IALG_Status *) &betaPrimeBase);
        acpAlg->fxns->algControl (acpAlg, ACP_GETBETAPRIMEOFFSET, 
            (IALG_Status *) &betaPrimeOffset);
        if (beta >= betaPrimeBase)
            beta += (stream - 1) * betaPrimeOffset;
    }
    
    this->next = NULL;
    this->code = pInit->thisCode;

    if (pInit->ialg_fxns) {

        if (pInit->init_func)
            (*pInit->init_func)();

        if (! (alg = PAF_ALG_create(pInit->ialg_fxns,NULL,(IALG_Params *)pInit->ialg_prms,
                     common, pafConfig))) {
            if (log) {
                LOG_printf(log,
                    "AS%d: beta 0x%x algorithm instance creation failed",
                    stream, beta);
            }
            return NULL;
        }
    }
    else
        return NULL;

    this->alg = (ASP_Handle )alg;

    if (alg->fxns->algControl) {
        if (! alg->fxns->algControl (alg, ACP_GETSTATUSADDRESS1, (IALG_Status *)&pStatus))
            acp->fxns->attach (acp, pInit->thisCode.part.series, beta, pStatus);
        if (! alg->fxns->algControl (alg, ACP_GETSTATUSADDRESS2, (IALG_Status *)&pStatus))
            acp->fxns->attach (acp, pInit->thisCode.part.series, beta+1, pStatus);
    }

    if (log)
        LOG_printf(log, "AS%d: beta 0x%x processing initialized", stream, beta);

    return this;
}

//
// Audio Stream Processing Chain Function - Link Copy
//
//   Name:      PAF_ASP_linkCopy
//   Purpose:   Copy the data for a chain link:
//              (1) Find an element on a chain, and
//              (2) Copy the reference to the data of the link.
//   From:      AST Parameter Chain Function -> linkCopy
//   Uses:      See code.
//   States:    None.
//   Return:    Null pointer on failure.
//              Pointer to the chain link on success.
//   Trace:     None.
//

PAF_ASP_Link *
PAF_ASP_linkCopy (
    PAF_ASP_Link *this,
    const PAF_ASP_LinkInit *pInit,
    PAF_ASP_Link *from)
{
    for ( ; from; from = from->next) {
        if (from->code.full == pInit->thisCode.full)
            break;
    }

    if (! from)
        return NULL;

    this->next = NULL;
    this->code = pInit->thisCode;
    this->alg = from->alg;

    return this;
}

//
// Audio Stream Processing Chain Functions
//
//   Name:      PAF_ASP_chainFxns
//   Purpose:   Collect the chain functions that embody the implementation 
//              of Audio Framework Number 2 for use as a jump table.
//   From:      PAF_AST_Params
//   Uses:      See contents.
//   States:    N.A.
//   Return:    N.A.
//   Trace:     None.
//

const struct PAF_ASP_ChainFxns PAF_ASP_chainFxns =
{
    PAF_ASP_chainInit_,
    PAF_ASP_chainLink_,
    PAF_ASP_chainFind_,
    {
        PAF_ASP_chainReset_,
        PAF_ASP_chainApply_,
        PAF_ASP_chainFinal_,
    },
    PAF_ASP_linkData,
    PAF_ASP_linkCopy,
};