TDA4VM: Restart demo failed and report errors when the demo exit abnormally

/*
 *
 * Copyright (c) 2020 Texas Instruments Incorporated
 *
 * All rights reserved not granted herein.
 *
 * Limited License.
 *
 * Texas Instruments Incorporated grants a world-wide, royalty-free, non-exclusive
 * license under copyrights and patents it now or hereafter owns or controls to make,
 * have made, use, import, offer to sell and sell ("Utilize") this software subject to the
 * terms herein.  With respect to the foregoing patent license, such license is granted
 * solely to the extent that any such patent is necessary to Utilize the software alone.
 * The patent license shall not apply to any combinations which include this software,
 * other than combinations with devices manufactured by or for TI ("TI Devices").
 * No hardware patent is licensed hereunder.
 *
 * Redistributions must preserve existing copyright notices and reproduce this license
 * (including the above copyright notice and the disclaimer and (if applicable) source
 * code license limitations below) in the documentation and/or other materials provided
 * with the distribution
 *
 * Redistribution and use in binary form, without modification, are permitted provided
 * that the following conditions are met:
 *
 * *       No reverse engineering, decompilation, or disassembly of this software is
 * permitted with respect to any software provided in binary form.
 *
 * *       any redistribution and use are licensed by TI for use only with TI Devices.
 *
 * *       Nothing shall obligate TI to provide you with source code for the software
 * licensed and provided to you in object code.
 *
 * If software source code is provided to you, modification and redistribution of the
 * source code are permitted provided that the following conditions are met:
 *
 * *       any redistribution and use of the source code, including any resulting derivative
 * works, are licensed by TI for use only with TI Devices.
 *
 * *       any redistribution and use of any object code compiled from the source code
 * and any resulting derivative works, are licensed by TI for use only with TI Devices.
 *
 * Neither the name of Texas Instruments Incorporated nor the names of its suppliers
 *
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * DISCLAIMER.
 *
 * THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "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 TI AND TI'S LICENSORS 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.
 *
 */

#include <utils/draw2d/include/draw2d.h>
#include <utils/perf_stats/include/app_perf_stats.h>
#include <utils/console_io/include/app_get.h>
#include <utils/grpx/include/app_grpx.h>
#include <VX/vx_khr_pipelining.h>
#include <signal.h>

#include "app_common.h"
#include "app_sensor_module.h"
#include "app_capture_module.h"
#include "app_viss_module.h"
#include "app_aewb_module.h"
#include "app_ldc_module.h"
#include "app_img_mosaic_module.h"
#include "app_display_module.h"

// #include "app_csitx_module.h"

#define APP_BUFFER_Q_DEPTH (4)
#define APP_PIPELINE_DEPTH (7)

#define AVM_OUTPUT_WIDTH (1600U)
#define AVM_OUTPUT_HEIGHT (2000U)

typedef struct {
    vx_node node;

    vx_user_data_object config;
    tivx_csitx_params_t params;
    vx_object_array csitx_image_arr[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_int32 graph_parameter_index;

}CsitxObj;

typedef struct
{
    CsitxObj csitxObj;

    /* OpenVX references */
    vx_context context;
    vx_graph graph;

    tivx_task task;
    vx_uint32 stop_task;
    vx_uint32 stop_task_done;

    app_perf_point_t total_perf;
    app_perf_point_t fileio_perf;
    app_perf_point_t draw_perf;

    int32_t pipeline;

    int32_t enqueueCnt;
    int32_t dequeueCnt;

} AppObj;

AppObj gAppObj;

static vx_status app_init(AppObj *obj);
static void app_deinit(AppObj *obj);
static vx_status app_create_graph(AppObj *obj);
static vx_status app_verify_graph(AppObj *obj);
static vx_status app_run_graph(AppObj *obj);
static vx_status app_run_graph_interactive(AppObj *obj);
static void app_delete_graph(AppObj *obj);
static void app_default_param_set(AppObj *obj);
static void app_pipeline_params_defaults(AppObj *obj);
static void add_graph_parameter_by_node_index(vx_graph graph, vx_node node, vx_uint32 node_parameter_index);
static vx_status set_tx_pattern(AppObj *obj);

static char menu[] = {
    "\n"
    "\n ========================="
    "\n Demo : Camera Demo"
    "\n ========================="
    "\n"
    "\n s: Save CSIx, VISS and LDC outputs"
    "\n"
    "\n p: Print performance statistics"
    "\n"
    "\n x: Exit"
    "\n"
    "\n Enter Choice: "};

static void app_run_task(void *app_var)
{
    AppObj *obj = (AppObj *)app_var;
    vx_status status = VX_SUCCESS;
    while ((!obj->stop_task) && (status == VX_SUCCESS))
    {
        status = app_run_graph(obj);
    }
    obj->stop_task_done = 1;
}

static int32_t app_run_task_create(AppObj *obj)
{
    tivx_task_create_params_t params;
    vx_status status;

    tivxTaskSetDefaultCreateParams(&params);
    params.task_main = app_run_task;
    params.app_var = obj;

    obj->stop_task_done = 0;
    obj->stop_task = 0;

    status = tivxTaskCreate(&obj->task, &params);

    return status;
}

static void app_run_task_delete(AppObj *obj)
{
    while (obj->stop_task_done == 0)
    {
        tivxTaskWaitMsecs(100);
    }

    tivxTaskDelete(&obj->task);
}

void handler(int signo)
{
    gAppObj.stop_task = 1;
}

vx_int32 app_multi_cam_main(vx_int32 argc, vx_char *argv[])
{
    signal(SIGINT, handler);
    
    uint32_t i = 0;
    vx_status status = VX_SUCCESS;
    for (i = 0; i < 1; i++) {
    status = VX_SUCCESS;

    AppObj *obj = &gAppObj;

    /*Optional parameter setting*/
    app_default_param_set(obj);
    if (status == VX_SUCCESS)
    {
        status = app_init(obj);

    }
    if (status == VX_SUCCESS)
    {
        // APP_PRINTF("App Init Done! \n");

        status = app_create_graph(obj);

        if (status == VX_SUCCESS)
        {
            // APP_PRINTF("App Create Graph Done! \n");

            status = app_verify_graph(obj);

            if (status == VX_SUCCESS)
            {
                // APP_PRINTF("App Verify Graph Done! \n");

                if (status == VX_SUCCESS)
                {
                    // APP_PRINTF("App Send Error Frame Done! \n");
                    status = app_run_graph(obj);
                }
            }
        }

        APP_PRINTF("App Run Graph Done! \n");
    }

    app_delete_graph(obj);

    APP_PRINTF("App Delete Graph Done! \n");

    app_deinit(obj);

    APP_PRINTF("App De-init Done! \n");
    }

    return status;
}

static vx_status app_init(AppObj *obj)
{
    vx_status status = VX_SUCCESS;
    uint32_t loopCnt;

    /* Create OpenVx Context */
    obj->context = vxCreateContext();
    status = vxGetStatus((vx_reference)obj->context);
    APP_PRINTF("Creating context done!\n");
    if (status == VX_SUCCESS)
    {
        tivxHwaLoadKernels(obj->context);
        tivxImagingLoadKernels(obj->context);
        tivxFileIOLoadKernels(obj->context);
        APP_PRINTF("Kernel loading done!\n");
    }

    /* Initialize modules */

    if (status == VX_SUCCESS)
    {
        // status = app_init_csitx(obj->context, &obj->csitxObj, AVM_OUTPUT_WIDTH, AVM_OUTPUT_HEIGHT);
        /* CSITX Config initialization */
        tivx_csitx_params_init(&obj->csitxObj.params);
        obj->csitxObj.params.numInst = 1U;
        obj->csitxObj.params.numCh = 1U;
        obj->csitxObj.params.instId[0U] = 0U;
        obj->csitxObj.params.instCfg[0U].rxCompEnable = (uint32_t)vx_true_e;
        obj->csitxObj.params.instCfg[0U].rxv1p3MapEnable = (uint32_t)vx_true_e;
        obj->csitxObj.params.instCfg[0U].laneBandSpeed = 0x05U; //CSITX_LANE_BAND_SPEED_240_TO_320_MBPS
        obj->csitxObj.params.instCfg[0U].numDataLanes = 4U;
        printf("laneBandSpeed %u, numDataLanes %u\n", obj->csitxObj.params.instCfg[0U].laneBandSpeed, obj->csitxObj.params.instCfg[0U].numDataLanes);
        for (loopCnt = 0U;
            loopCnt < obj->csitxObj.params.instCfg[0U].numDataLanes;
            loopCnt++)
        {
            obj->csitxObj.params.instCfg[0U].lanePolarityCtrl[loopCnt] = 0u;
        }
        for (loopCnt = 0U; loopCnt < 1U; loopCnt++)
        {
            obj->csitxObj.params.chVcNum[loopCnt] = loopCnt;
            obj->csitxObj.params.chInstMap[loopCnt] = 0U;
        }

        obj->csitxObj.config = vxCreateUserDataObject(obj->context, "tivx_csitx_params_t", sizeof(tivx_csitx_params_t), &obj->csitxObj.params);

        vx_image cap_yuv_image = vxCreateImage(obj->context, AVM_OUTPUT_WIDTH, AVM_OUTPUT_HEIGHT, VX_DF_IMAGE_UYVY); // PDCU-AVM 1600*2000
        status = vxGetStatus((vx_reference)cap_yuv_image);
        uint32_t q;
        if (status == VX_SUCCESS)
        {
            for (q = 0; q < APP_MODULES_MAX_BUFQ_DEPTH; q++)
            {
                obj->csitxObj.csitx_image_arr[q] = vxCreateObjectArray(obj->context, (vx_reference)cap_yuv_image, 1);
                status = vxGetStatus((vx_reference)obj->csitxObj.csitx_image_arr[q]);
            }
        }
        APP_PRINTF("csitx init done!\n");
    }

    appPerfPointSetName(&obj->total_perf, "TOTAL");
    appPerfPointSetName(&obj->fileio_perf, "FILEIO");
    return status;
}

static void app_deinit(AppObj *obj)
{
    // vx_status deinit_st = 0;
    // deinit_st = app_deinit_csitx(&obj->csitxObj);
    uint32_t i;
    for (i = 0; i < APP_MODULES_MAX_BUFQ_DEPTH; i++)
    {
        vxReleaseObjectArray(&obj->csitxObj.csitx_image_arr[i]);
    }
    vxReleaseUserDataObject(&obj->csitxObj.config);
    APP_PRINTF("Csitx deinit done!\n");

    tivxHwaUnLoadKernels(obj->context);
    tivxImagingUnLoadKernels(obj->context);
    tivxFileIOUnLoadKernels(obj->context);
    APP_PRINTF("Kernels unload done!\n");

    vxReleaseContext(&obj->context);
    APP_PRINTF("Release context done!\n");
}

static void app_delete_graph(AppObj *obj)
{
    // vx_status delete_st = 0;
    // delete_st = app_delete_csitx(&obj->csitxObj);
    if (obj->csitxObj.node != NULL)
    {
        vxReleaseNode(&obj->csitxObj.node);
    }
    APP_PRINTF("Csitx delete done!\n");

    vxReleaseGraph(&obj->graph);
    APP_PRINTF("Graph delete done!\n");
}

static vx_status app_create_graph(AppObj *obj)
{
    vx_status status = VX_SUCCESS;
    vx_graph_parameter_queue_params_t graph_parameters_queue_params_list[2];
    vx_int32 graph_parameter_index;

    obj->graph = vxCreateGraph(obj->context);
    status = vxGetStatus((vx_reference)obj->graph);
    if (status == VX_SUCCESS)
    {
        status = vxSetReferenceName((vx_reference)obj->graph, "app_multi_cam_graph");
        APP_PRINTF("Graph create done!\n");
    }

    status = set_tx_pattern(obj);
    if (status != VX_SUCCESS)
        printf("\nset tx pattern error!\n\n");
    // status = app_create_graph_csitx(obj->graph, &obj->csitxObj);
    if ((vx_true_e == tivxIsTargetEnabled(TIVX_TARGET_CSITX)))
    {
        obj->csitxObj.node = tivxCsitxNode(obj->graph, obj->csitxObj.config, obj->csitxObj.csitx_image_arr[0]);
        status = vxGetStatus((vx_reference)obj->csitxObj.node);

        if (status == VX_SUCCESS)
        {
            vxSetNodeTarget(obj->csitxObj.node, VX_TARGET_STRING, TIVX_TARGET_CSITX);
            vxSetReferenceName((vx_reference)obj->csitxObj.node, "CSITXNode");
        }
        else
        {
            printf("[CSITX-MODULE] Unable to create CSITX node!\n");
        }
    }
    else
    {
        printf("[CSITX-MODULE] Unable to create CSITX node, TIVX_TARGET_CSITX not enable!\n");
    }
    printf("Csitx graph done!\n");

    if (status == VX_SUCCESS)
    {
        graph_parameter_index = 0;
        add_graph_parameter_by_node_index(obj->graph, obj->csitxObj.node, 1);
        obj->csitxObj.graph_parameter_index = graph_parameter_index;
        graph_parameters_queue_params_list[graph_parameter_index].graph_parameter_index = graph_parameter_index;
        graph_parameters_queue_params_list[graph_parameter_index].refs_list_size = APP_BUFFER_Q_DEPTH;
        graph_parameters_queue_params_list[graph_parameter_index].refs_list = (vx_reference *)&obj->csitxObj.csitx_image_arr[0];
        graph_parameter_index++;

        status = vxSetGraphScheduleConfig(obj->graph,
                                          VX_GRAPH_SCHEDULE_MODE_QUEUE_AUTO,
                                          graph_parameter_index,
                                          graph_parameters_queue_params_list);

        if (status == VX_SUCCESS)
        {
            status = tivxSetGraphPipelineDepth(obj->graph, APP_PIPELINE_DEPTH);
        }
    }

    return status;
}

static vx_status app_verify_graph(AppObj *obj)
{
    vx_status status = VX_SUCCESS;

    status = vxVerifyGraph(obj->graph);

    if (status == VX_SUCCESS)
    {
        APP_PRINTF("Graph verify done!\n");
    }

#if 0
    if(VX_SUCCESS == status)
    {
      status = tivxExportGraphToDot(obj->graph,".", "vx_app_multi_cam");
    }
#endif

    /* wait a while for prints to flush */
    tivxTaskWaitMsecs(100);

    return status;
}

static vx_status app_run_graph_for_one_frame_pipeline(AppObj *obj, vx_int32 frame_id)
{
    vx_status status = VX_SUCCESS;

    //APP_PRINTF("app_run_graph_for_one_pipeline: frame %d beginning\n", frame_id);
    appPerfPointBegin(&obj->total_perf);
    CsitxObj *csitxObj = &obj->csitxObj;

    if (obj->pipeline < 0)
    {
        /* Enqueue outpus */

        /* Enqueue inputs during pipeup dont execute */
        if (status == VX_SUCCESS)
        {
            status = vxGraphParameterEnqueueReadyRef(obj->graph, csitxObj->graph_parameter_index, (vx_reference *)&obj->csitxObj.csitx_image_arr[obj->enqueueCnt], 1);
        }
        obj->enqueueCnt++;
        obj->enqueueCnt = (obj->enqueueCnt >= APP_BUFFER_Q_DEPTH) ? 0 : obj->enqueueCnt;
        obj->pipeline++;
    }

    if ((obj->pipeline == 0) && (status == VX_SUCCESS))
    {
        if (status == VX_SUCCESS)
        {
            status = vxGraphParameterEnqueueReadyRef(obj->graph, csitxObj->graph_parameter_index, (vx_reference *)&obj->csitxObj.csitx_image_arr[obj->enqueueCnt], 1);
        }
        obj->enqueueCnt++;
        obj->enqueueCnt = (obj->enqueueCnt >= APP_BUFFER_Q_DEPTH) ? 0 : obj->enqueueCnt;
        obj->pipeline++;
    }

    if ((obj->pipeline > 0) && (status == VX_SUCCESS))
    {
        vx_object_array csitx_arr;
        uint32_t num_refs;

        /* Dequeue input */
        status = vxGraphParameterDequeueDoneRef(obj->graph, csitxObj->graph_parameter_index, (vx_reference *)&csitx_arr, 1, &num_refs);

        /* Enqueue input - start execution */
        if (status == VX_SUCCESS)
        {
            status = vxGraphParameterEnqueueReadyRef(obj->graph, csitxObj->graph_parameter_index, (vx_reference *)&csitx_arr, 1);
        }
        obj->enqueueCnt++;
        obj->dequeueCnt++;

        obj->enqueueCnt = (obj->enqueueCnt >= APP_BUFFER_Q_DEPTH) ? 0 : obj->enqueueCnt;
        obj->dequeueCnt = (obj->dequeueCnt >= APP_BUFFER_Q_DEPTH) ? 0 : obj->dequeueCnt;
    }

    appPerfPointEnd(&obj->total_perf);
    return status;
}

static vx_status app_run_graph_interactive(AppObj *obj)
{
    vx_status status;
    uint32_t done = 0;

    char ch;
    FILE *fp;
    app_perf_point_t *perf_arr[1];

    status = app_run_task_create(obj);
    if (status == VX_FAILURE)
    {
        printf("app_tidl: ERROR: Unable to create task\n");
    }
    else
    {
        appPerfStatsResetAll();
        while (!done)
        {
            printf(menu);
            ch = getchar();
            printf("\n");

            switch (ch)
            {
            case 'p':
                appPerfStatsPrintAll();
                status = tivx_utils_graph_perf_print(obj->graph);
                appPerfPointPrint(&obj->fileio_perf);
                appPerfPointPrint(&obj->total_perf);
                printf("\n");
                appPerfPointPrintFPS(&obj->total_perf);
                appPerfPointReset(&obj->total_perf);
                printf("\n");

                vx_reference refs[1];
                refs[0] = (vx_reference)(obj->csitxObj.csitx_image_arr[0]);
                if (status == VX_SUCCESS)
                {
                    status = tivxNodeSendCommand(obj->csitxObj.node, 0u,
                                                 TIVX_CSITX_PRINT_STATISTICS,
                                                 refs, 1u);
                }
                break;
            case 'e':
                perf_arr[0] = &obj->total_perf;
                fp = appPerfStatsExportOpenFile(".", "basic_demos_app_multi_cam");
                if (NULL != fp)
                {
                    appPerfStatsExportAll(fp, perf_arr, 1);
                    if (status == VX_SUCCESS)
                    {
                        status = tivx_utils_graph_perf_export(fp, obj->graph);
                    }
                    appPerfStatsExportCloseFile(fp);
                    appPerfStatsResetAll();
                }
                else
                {
                    printf("fp is null\n");
                }
                break;
            case 'x':
                obj->stop_task = 1;
                done = 1;
                break;
            }
        }
        app_run_task_delete(obj);
    }
    return status;
}

static vx_status app_run_graph(AppObj *obj)
{
    vx_status status = VX_SUCCESS;

    vx_int32 frame_id;
    FILE *fp;
    app_perf_point_t *perf_arr[1];

    app_pipeline_params_defaults(obj);
    APP_PRINTF("app_pipeline_params_defaults returned\n");

    for (frame_id = 0; frame_id < 0; frame_id++)
    {

        if (status == VX_SUCCESS)
        {
            status = app_run_graph_for_one_frame_pipeline(obj, frame_id);
        }

        /* user asked to stop processing */
        if (obj->stop_task)
            break;
    }
    perf_arr[0] = &obj->total_perf;
    fp = appPerfStatsExportOpenFile("/ramdisk", "pdcu_avm_perf");
    if (NULL != fp)
    {
        appPerfStatsExportHirain(fp, perf_arr, 1);
        printf("PerfStatsExportHirain is finished \n");
        appPerfStatsExportCloseFile(fp);
        printf("PerfStatsExportCloseFile is finished \n");
        appPerfStatsResetAll();
        printf("PerfStatsResetAll is finished \n");
    }
    else
    {
        printf("fp is null\n");
    }

    if (status == VX_SUCCESS)
    {
        status = vxWaitGraph(obj->graph);
        printf("WaitGraph is finished \n");
    }

    return status;
}

static void app_pipeline_params_defaults(AppObj *obj)
{
    obj->pipeline = -APP_BUFFER_Q_DEPTH + 1;
    obj->enqueueCnt = 0;
    obj->dequeueCnt = 0;
}

static void app_default_param_set(AppObj *obj)
{
    app_pipeline_params_defaults(obj);
}

/*
 * Utility API used to add a graph parameter from a node, node parameter index
 */
static void add_graph_parameter_by_node_index(vx_graph graph, vx_node node, vx_uint32 node_parameter_index)
{
    vx_parameter parameter = vxGetParameterByIndex(node, node_parameter_index);

    vxAddParameterToGraph(graph, parameter);
    vxReleaseParameter(&parameter);
}

static vx_status set_tx_pattern(AppObj *obj)
{
    vx_image tx_out_image[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_status status = VX_FAILURE;
    vx_uint32 width[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_uint32 height[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_imagepatch_addressing_t image_addr[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_rectangle_t rect[APP_MODULES_MAX_BUFQ_DEPTH];
    vx_map_id map_id[APP_MODULES_MAX_BUFQ_DEPTH];
    void *data_ptr[APP_MODULES_MAX_BUFQ_DEPTH];

    for (int i = 0; i < APP_MODULES_MAX_BUFQ_DEPTH; i++)
    {
        tx_out_image[i] = (vx_image)vxGetObjectArrayItem(obj->csitxObj.csitx_image_arr[i], 0);
        status = vxGetStatus((vx_reference)tx_out_image[i]);
        if (status != VX_SUCCESS)
        {
            printf("tx_out_image %d status error \n", i);
            return VX_FAILURE;
        }
        vxQueryImage(tx_out_image[i], VX_IMAGE_WIDTH, &width[i], sizeof(vx_uint32));
        vxQueryImage(tx_out_image[i], VX_IMAGE_HEIGHT, &height[i], sizeof(vx_uint32));
        rect[i].start_x = 0;
        rect[i].start_y = 0;
        rect[i].end_x = width[i];
        rect[i].end_y = height[i];

        status = vxMapImagePatch(tx_out_image[i],
                                 &rect[i],
                                 0,
                                 &map_id[i],
                                 &image_addr[i],
                                 &data_ptr[i],
                                 VX_READ_AND_WRITE,
                                 VX_MEMORY_TYPE_HOST,
                                 VX_NOGAP_X);

        if (!data_ptr[i])
        {
            printf("data_ptr is NULL \n");
            return VX_FAILURE;
        }

        memset(data_ptr[i], 0xAB, width[i] * height[i] * 2);

        // imgaddr_stride = image_addr.stride_y;
        vxUnmapImagePatch(tx_out_image[i], map_id[i]);
    }

    return VX_SUCCESS;
}