#include "Calibration.h"
#include "Line.h"

#define MODULE_NAME "AVM_Calibration"
#define AVM_DEBUG_SWITCH    1
#include "Avm_log.h"


static CameraParamEXT *g_stCameraExtPara = NULL;
static double g_CameraIntPara[8];
static Corner *g_SaleCorner = NULL;



/*****************functions Lev 2**********************************/
// copy from C35DB
void TvsLibSetDefaultParam(OVSBasicInfo * pOVSBasicInfo)
{
    float A = 0.0;
    float B = 0.0;
    float C = 0.0;

    /* initialize camera internal parameters */
    int k;
    for (k=0; k<4; k++)
    {
        pOVSBasicInfo->stCameraParamINT[k].iWidth = 1280;
        pOVSBasicInfo->stCameraParamINT[k].iHeight = 720;
        pOVSBasicInfo->stCameraParamINT[k].iDirectMappingCount = 5;
        pOVSBasicInfo->stCameraParamINT[k].daDirectMapping[0] = -341.7126363462;
        pOVSBasicInfo->stCameraParamINT[k].daDirectMapping[1] = 0.0000000000;
        pOVSBasicInfo->stCameraParamINT[k].daDirectMapping[2] = 0.0007684678;
        pOVSBasicInfo->stCameraParamINT[k].daDirectMapping[3] = -0.0000002661;
        pOVSBasicInfo->stCameraParamINT[k].daDirectMapping[4] = 0.0000000006;
        pOVSBasicInfo->stCameraParamINT[k].iInverseMappingCount= 9;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[0] = 640.6412457045;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[1] = 509.1525491144;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[2] = 61.2965144805;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[3] = 10.7887547597;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[4] = 60.3102715601;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[5] = 59.2976199022;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[6] = 17.0524971165;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[7] = -1.3699310356;
        pOVSBasicInfo->stCameraParamINT[k].daInverseMapping[8] = -1.0491373923;
        pOVSBasicInfo->stCameraParamINT[k].daIntParam[0] = 0.9996316191;
        pOVSBasicInfo->stCameraParamINT[k].daIntParam[1] = 0.0000295379;
        pOVSBasicInfo->stCameraParamINT[k].daIntParam[2] = -0.0000254068;
        pOVSBasicInfo->stCameraParamINT[k].daValidRange[0] = 1280;
        pOVSBasicInfo->stCameraParamINT[k].daValidRange[1] = 720;
        pOVSBasicInfo->stCameraParamINT[k].daValidRange[2] = 0;
        pOVSBasicInfo->stCameraParamINT[k].daValidRange[3] = 0;
    }

#if 0
    pOVSBasicInfo->stBirdView.iaTVSArea[0] = 2000;
    pOVSBasicInfo->stBirdView.iaTVSArea[1] = 5000;
    pOVSBasicInfo->stBirdView.iaTVSArea[2] = 4120;
    pOVSBasicInfo->stBirdView.iaTVSArea[3] = 4000;
    pOVSBasicInfo->stBirdView.iaTVSArea[4] = 700;
    pOVSBasicInfo->stBirdView.iaTVSArea[5] = 700;
    pOVSBasicInfo->stBirdView.iaTVSArea[6] = 4000;
    pOVSBasicInfo->stBirdView.iaTVSArea[7] = 10240;
    pOVSBasicInfo->stBirdView.iaTVSArea[8] = 14400;
    pOVSBasicInfo->stBirdView.iImgHeight = 720;
    pOVSBasicInfo->stBirdView.iImgWidth = 512;
    pOVSBasicInfo->stBirdView.iVehicleImgIdx = 0;

    pOVSBasicInfo->stLdwParam.iImgCenterU = 128;
    pOVSBasicInfo->stLdwParam.iImgCenterV = 180;
    pOVSBasicInfo->stLdwParam.iImgHeight = 360;
    pOVSBasicInfo->stLdwParam.iImgWidth = 256;
    pOVSBasicInfo->stLdwParam.iRatioU = 40;
    pOVSBasicInfo->stLdwParam.iRatioV = 40;
    pOVSBasicInfo->stLdwParam.iImgVehicleHeight = 125;
    pOVSBasicInfo->stLdwParam.iImgVehicleWidth = 50;
    pOVSBasicInfo->stLdwParam.iImgVehicleCoordU = 103;
    pOVSBasicInfo->stLdwParam.iImgVehicleCoordV = 118;
#endif
    pOVSBasicInfo->stVehicle.fAckermanCoe = 1.0 / 18;
    pOVSBasicInfo->stVehicle.fMaxLeftAngle = -554;
    pOVSBasicInfo->stVehicle.fMaxRightAngle = 554;
    pOVSBasicInfo->stVehicle.iaTread[0] = 1535/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iaTread[1] = 1535/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iFrontOverhang = 880/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iRearOverhang = 720/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iVehicleLength = 4175/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iVehicleWidth = 1769/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iWheelBase = 2575/LAB_SCALE;
    pOVSBasicInfo->stVehicle.iWheelWidth = 219;

    pOVSBasicInfo->stUndistView.iRangeWidth[0] = 450;
    pOVSBasicInfo->stUndistView.iRangeHeight[0] = 260;
    pOVSBasicInfo->stUndistView.iRangeWidth[1] = 450;
    pOVSBasicInfo->stUndistView.iRangeHeight[1] = 260;
    pOVSBasicInfo->stUndistView.iImgWidth =800;
    pOVSBasicInfo->stUndistView.iImgHeight = 587;
    pOVSBasicInfo->stUndistView.faRangeOffsetRatio[0] = -0.02;
    pOVSBasicInfo->stUndistView.faRangeOffsetRatio[1] =  0.00;
    pOVSBasicInfo->stUndistView.faRangeOffsetRatio[2] = -0.05;
    pOVSBasicInfo->stUndistView.faRangeOffsetRatio[3] =  0.00;
    pOVSBasicInfo->stUndistView.iaViewAngle[0] = 145;
    pOVSBasicInfo->stUndistView.iaViewAngle[1] = 130;
    pOVSBasicInfo->stUndistView.iaViewAngle[2] = 145;
    pOVSBasicInfo->stUndistView.iaViewAngle[3] = 130;
    pOVSBasicInfo->stUndistView.faVirCamFOVandDIR[0][0]=104; //right    //11-23
    pOVSBasicInfo->stUndistView.faVirCamFOVandDIR[1][0]=104; //left

    pOVSBasicInfo->stTracksAndAssist.iaDynaTrackStartDist[0] = 100;
    pOVSBasicInfo->stTracksAndAssist.iaDynaTrackStartDist[1] = 200;
    pOVSBasicInfo->stTracksAndAssist.iDynaTrackLen = 3500;
    pOVSBasicInfo->stTracksAndAssist.iStartToFrontWheel = pOVSBasicInfo->stVehicle.iFrontOverhang+300;
    pOVSBasicInfo->stTracksAndAssist.iStartToRearWheel = pOVSBasicInfo->stVehicle.iRearOverhang+300;

    A = 1.38500;
    B = 3.26790;
    C = 2.3;
    pOVSBasicInfo->stBowlVerTexParam.iBottomLayerNum = 20;
    pOVSBasicInfo->stBowlVerTexParam.iWallLayerNum = 4;
    pOVSBasicInfo->stBowlVerTexParam.k = (B-C)/A;
    pOVSBasicInfo->stBowlVerTexParam.b = C;
    pOVSBasicInfo->stBowlVerTexParam.iRatio = 700;
    pOVSBasicInfo->stBowlVerTexParam.fLowX = 1.2;
    pOVSBasicInfo->stBowlVerTexParam.fLowY = 3.0;
    pOVSBasicInfo->stBowlVerTexParam.fHighX = 6.0;
    pOVSBasicInfo->stBowlVerTexParam.fHighY = 10.0;
    pOVSBasicInfo->stBowlVerTexParam.iPointNum = 25;
    pOVSBasicInfo->stBowlVerTexParam.iPointNumLR = 50;
}

/*读取luts文件中的摄像头内外参来替换默认参数
* 当文件读取失败时，则使用默认参数
* return: 0:内外参都使用luts数据；
*         1:外参数据使用默认参数， 内参数据使用luts数据
*         2:外参数据使用luts参数， 内参数据使用默认数据
*         3:内外参都使用默认数据；
*/
int TvsLibSetLutsParam(char *dirLuts, CameraParamEXT *pCameraParamEXT, CameraParamINT *pCameraParamINT)
{
    int k = 0;
    int ret, ret1, ret2;
    int readsize = 0;
    CameraParamEXT *tmpExtParams = NULL;
    double *tmpIntParams = NULL;

    do
    {
        AVM_DEBUG("load EXT params\n");
        if (NULL == tmpExtParams)
        {
            tmpExtParams = (CameraParamEXT *)malloc(4*sizeof(CameraParamEXT));
        }
        readsize = LoadFile(dirLuts, LUTS_CAMERA_EXT, (UChar**)&tmpExtParams, 4*sizeof(CameraParamEXT));
        if (4*sizeof(CameraParamEXT) != readsize)
        {
            AVM_ERROR("%s:file size dosen't match ExtParam struct, filesize=%d, sizeof(CameraParamEXT)=%d\n", readsize, 4*sizeof(CameraParamEXT));
            ret1 = 1;
            break;
        }
        memcpy((void *)pCameraParamEXT, tmpExtParams, sizeof(CameraParamEXT)*4);
        ret1 = 0;
    } while(0);

    do
    {
        if (NULL == tmpIntParams)
        {
            tmpIntParams = (double *)malloc(8*sizeof(double));
        }
        readsize = LoadFile(dirLuts, LUTS_CAMERA_INT, (UChar**)&tmpIntParams, 8*sizeof(double));
        if (8*sizeof(double) != readsize)
        {
            AVM_ERROR("%s:file size dosen't match IntParam struct, filesize=%d, sizeof(CameraParamEXT)=%d\n", readsize, 8*sizeof(double));
            ret2 = 2;
            break;
        }
        for (k = 0; k < 4; k++)
        {
            pCameraParamINT[k].daIntParam[3] = tmpIntParams[2*k];
            pCameraParamINT[k].daIntParam[4] = tmpIntParams[2*k+1];
        }
        ret2 = 0;
    } while(0);

    if (NULL != tmpExtParams)
    {
        free(tmpExtParams);
        tmpExtParams = NULL;
    }
    if (NULL != tmpIntParams)
    {
        free(tmpIntParams);
        tmpIntParams = NULL;
    }
    ret = ret1 + ret2;
    return ret;
}

/*****************functions Lev 1**********************************/
void AlgParamInit(char *dirLuts, OVSBasicInfo *pOVSBasicInfo)
{
    do
    {
        //设置全部算法相关的初始参数
        TvsLibSetDefaultParam(pOVSBasicInfo);

        //读取luts文件中的摄像头内外参来替换默认参数
        TvsLibSetLutsParam(dirLuts, pOVSBasicInfo->stCameraParamEXT, pOVSBasicInfo->stCameraParamINT);
    } while(0);
}

void ApicaliInit()
{
}


bool avmDefaultCorner(Corner *stCorner)
{
    bool ret = FALSE;

    do
    {
        memcpy(stCorner, g_SaleCorner, sizeof(Corner));

        ret = TRUE;
    } while(0);

    return ret;
}


/*
*获取角点坐标及内外参
*输入:stVideoList
*输出:outCorners
*/
void ApiOfflineCali(VideoList *stVideoList, Coord outCorners[4][23])
{
}


void ApiSaleCali(Corner* pCorner)
{
}


void ApiGenerateLuts()
{
}


int GenTrackFrontDynamic(OVSBasicInfo *pOVSBasicInfo, VehicleTracks *pVehicleTracks, int angle, int fr)
{
    int ret = -1;

    do
    {

        ret = 0;
    } while(0);

    return ret;
}


int GenTrackRearDynamic(OVSBasicInfo *pOVSBasicInfo, VehicleTracks *pVehicleTracks, int angle, int fr)
{
    int ret = -1;

    do
    {

        ret = 0;
    } while(0);

    return ret;
}


int GenFrontTrackDynaBirdImg(OVSBasicInfo *pOVSBasicInfo, BirdviewTrack *pBirdviewTrack, int angle)
{
    int ret = -1;

    do
    {

        ret = 0;
    } while(0);

    return ret;
}


int GenRearTrackDynaBirdImg(OVSBasicInfo *pOVSBasicInfo,BirdviewTrack *pBirdviewTrack, int angle)
{
    int ret = -1;

    do
    {

        ret = 0;
    } while(0);

    return ret;
}


int algWBCalculate(unsigned char **videoList, unsigned char camState, double *factor) //videolise[0-3]:front-rear-left-right;    camstate:bit0-bit3:front-rear-left-right;    factor[0-3]:front-rear-left-right
{
    int ret = -1;

    do
    {
        factor[0] = 1.0;
        factor[1] = 1.0;
        factor[2] = 1.0;
        factor[3] = 1.0;

        ret = 0;
    } while(0);

    return ret;
}


/*
*videioList:前-后-左-右
*camState:前-后-左-右
*factor:左-右-前-后
*/
int funWBCalculate(UChar **videoList, UChar camState, double *factor)
{
    double tmpA,tmpB;
    int ret = -1;

    if (0 == algWBCalculate(videoList, camState, factor))
    {
        ret = 0;
    }
    tmpA = factor[0];
    tmpB = factor[1];
    factor[0] = factor[2];
    factor[1] = factor[3];
    factor[2] = tmpA;
    factor[3] = tmpB;

    return ret;
}

