/*
 *  ar3DUtil.c
 *  ARToolKit5
 *
 *  This file is part of ARToolKit.
 *
 *  ARToolKit is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  ARToolKit is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with ARToolKit.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  As a special exception, the copyright holders of this library give you
 *  permission to link this library with independent modules to produce an
 *  executable, regardless of the license terms of these independent modules, and to
 *  copy and distribute the resulting executable under terms of your choice,
 *  provided that you also meet, for each linked independent module, the terms and
 *  conditions of the license of that module. An independent module is a module
 *  which is neither derived from nor based on this library. If you modify this
 *  library, you may extend this exception to your version of the library, but you
 *  are not obligated to do so. If you do not wish to do so, delete this exception
 *  statement from your version.
 *
 *  Copyright 2015 Daqri, LLC.
 *  Copyright 2003-2015 ARToolworks, Inc.
 *
 *  Author(s): Hirokazu Kato
 *
 */
/*******************************************************
 *
 * Author: Hirokazu Kato
 *
 *         kato@sys.im.hiroshima-cu.ac.jp
 *
 * Revision: 1.0
 * Date: 00/08/31
 *
 *******************************************************/

#include <stdlib.h>
#include <math.h>
#include <AR/ar.h>
#include <AR/icp.h>

ARdouble arGetStereoMatchingErrorSquare( AR3DStereoHandle *handle,
                                       ARMarkerInfo *marker_infoL, ARMarkerInfo *marker_infoR )
{
    ARdouble   pos2dL[4][2], pos2dR[4][2];
    ARdouble   err;
    int      dir;

    dir = marker_infoL->dir;
    pos2dL[0][0] = marker_infoL->vertex[(4-dir)%4][0];
    pos2dL[0][1] = marker_infoL->vertex[(4-dir)%4][1];
    pos2dL[1][0] = marker_infoL->vertex[(5-dir)%4][0];
    pos2dL[1][1] = marker_infoL->vertex[(5-dir)%4][1];
    pos2dL[2][0] = marker_infoL->vertex[(6-dir)%4][0];
    pos2dL[2][1] = marker_infoL->vertex[(6-dir)%4][1];
    pos2dL[3][0] = marker_infoL->vertex[(7-dir)%4][0];
    pos2dL[3][1] = marker_infoL->vertex[(7-dir)%4][1];
    dir = marker_infoR->dir;
    pos2dR[0][0] = marker_infoR->vertex[(4-dir)%4][0];
    pos2dR[0][1] = marker_infoR->vertex[(4-dir)%4][1];
    pos2dR[1][0] = marker_infoR->vertex[(5-dir)%4][0];
    pos2dR[1][1] = marker_infoR->vertex[(5-dir)%4][1];
    pos2dR[2][0] = marker_infoR->vertex[(6-dir)%4][0];
    pos2dR[2][1] = marker_infoR->vertex[(6-dir)%4][1];
    pos2dR[3][0] = marker_infoR->vertex[(7-dir)%4][0];
    pos2dR[3][1] = marker_infoR->vertex[(7-dir)%4][1];

    err = 0.0;
    err += arGetStereoMatchingError(handle, pos2dL[0], pos2dR[0]);
    err += arGetStereoMatchingError(handle, pos2dL[1], pos2dR[1]);
    err += arGetStereoMatchingError(handle, pos2dL[2], pos2dR[2]);
    err += arGetStereoMatchingError(handle, pos2dL[3], pos2dR[3]);
    err /= 4.0;

    return err;
}

ARdouble arGetStereoMatchingError( AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2] )
{
    ARMat   *cL, *cR, *cLi, *cRi;
    ARMat   *tL, *tR, *tLi, *tRi;
    ARMat   *w1, *w2, *w3;
    ARdouble  q1, q2, q3, t1, t2, t3;
    ARdouble  a1, b1, c1, a2, b2, c2;
    ARdouble  lL2, lR2;
    int     i, j;
                                                                                                   
    cL = arMatrixAlloc( 4, 4 );
    cR = arMatrixAlloc( 4, 4 );
    tL = arMatrixAlloc( 4, 4 );
    tR = arMatrixAlloc( 4, 4 );
    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            cL->m[j*4+i] = handle->icpStereoHandle->matXcl2Ul[j][i];
        }
    }
    cL->m[12] = cL->m[13] = cL->m[14] = 0.0; cL->m[15] = 1.0;
    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            cR->m[j*4+i] = handle->icpStereoHandle->matXcr2Ur[j][i];
        }
    }
    cR->m[12] = cR->m[13] = cR->m[14] = 0.0; cR->m[15] = 1.0;
    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            tL->m[j*4+i] = handle->icpStereoHandle->matC2L[j][i];
        }
    }
    tL->m[12] = tL->m[13] = tL->m[14] = 0.0; tL->m[15] = 1.0;
    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            tR->m[j*4+i] = handle->icpStereoHandle->matC2R[j][i];
        }
    }
    tR->m[12] = tR->m[13] = tR->m[14] = 0.0; tR->m[15] = 1.0;

    cLi = arMatrixAllocInv( cL );
    cRi = arMatrixAllocInv( cR );
    tLi = arMatrixAllocInv( tL );
    tRi = arMatrixAllocInv( tR );

    w1 = arMatrixAllocMul( cR, tR );
    w2 = arMatrixAllocMul( w1, tLi );
    w3 = arMatrixAllocMul( w2, cLi );
    q1 = w3->m[0*4+0]*pos2dL[0] + w3->m[0*4+1]*pos2dL[1] + w3->m[0*4+2];
    q2 = w3->m[1*4+0]*pos2dL[0] + w3->m[1*4+1]*pos2dL[1] + w3->m[1*4+2];
    q3 = w3->m[2*4+0]*pos2dL[0] + w3->m[2*4+1]*pos2dL[1] + w3->m[2*4+2];
    t1 = w3->m[0*4+3];
    t2 = w3->m[1*4+3];
    t3 = w3->m[2*4+3];
    a1 = q3*t2 - q2*t3;
    b1 = q1*t3 - q3*t1;
    c1 = q2*t1 - q1*t2;

    arMatrixMul( w1, cL, tL );
    arMatrixMul( w2, w1, tRi );
    arMatrixMul( w3, w2, cRi );
    q1 = w3->m[0*4+0]*pos2dR[0] + w3->m[0*4+1]*pos2dR[1] + w3->m[0*4+2];
    q2 = w3->m[1*4+0]*pos2dR[0] + w3->m[1*4+1]*pos2dR[1] + w3->m[1*4+2];
    q3 = w3->m[2*4+0]*pos2dR[0] + w3->m[2*4+1]*pos2dR[1] + w3->m[2*4+2];
    t1 = w3->m[0*4+3];
    t2 = w3->m[1*4+3];
    t3 = w3->m[2*4+3];
    a2 = q3*t2 - q2*t3;
    b2 = q1*t3 - q3*t1;
    c2 = q2*t1 - q1*t2;

    arMatrixFree( w3 );
    arMatrixFree( w2 );
    arMatrixFree( w1 );
    arMatrixFree( tL );
    arMatrixFree( tR );
    arMatrixFree( tLi );
    arMatrixFree( tRi );
    arMatrixFree( cR );
    arMatrixFree( cL );
    arMatrixFree( cRi );
    arMatrixFree( cLi );

    lL2 = (a1*pos2dR[0]+b1*pos2dR[1]+c1)*(a1*pos2dR[0]+b1*pos2dR[1]+c1) / (a1*a1+b1*b1);
    lR2 = (a2*pos2dL[0]+b2*pos2dL[1]+c2)*(a2*pos2dL[0]+b2*pos2dL[1]+c2) / (a2*a2+b2*b2);

    return (lL2 + lR2)/2.0;
}

int arGetStereoMatching(AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2], ARdouble pos3d[3])
{
    ARMat   *matP,  *matQ;
    ARMat   *matPt, *matR, *matS, *matT;
    ARdouble   wL[3][4], wR[3][4];

    arUtilMatMul( (const ARdouble (*)[4])(handle->icpStereoHandle->matXcl2Ul), (const ARdouble (*)[4])(handle->icpStereoHandle->matC2L), wL );
    arUtilMatMul( (const ARdouble (*)[4])(handle->icpStereoHandle->matXcr2Ur), (const ARdouble (*)[4])(handle->icpStereoHandle->matC2R), wR );

    matP  = arMatrixAlloc(4,3);
    matPt = arMatrixAlloc(3,4);
    matQ  = arMatrixAlloc(4,1);
    matR  = arMatrixAlloc(3,3);
    matS  = arMatrixAlloc(3,1);
    matT  = arMatrixAlloc(3,1);

    matP->m[0*3+0] = matPt->m[0*4+0] = wL[0][0] - wL[2][0] * pos2dL[0];
    matP->m[0*3+1] = matPt->m[1*4+0] = wL[0][1] - wL[2][1] * pos2dL[0];
    matP->m[0*3+2] = matPt->m[2*4+0] = wL[0][2] - wL[2][2] * pos2dL[0];
    matP->m[1*3+0] = matPt->m[0*4+1] = wL[1][0] - wL[2][0] * pos2dL[1];
    matP->m[1*3+1] = matPt->m[1*4+1] = wL[1][1] - wL[2][1] * pos2dL[1];
    matP->m[1*3+2] = matPt->m[2*4+1] = wL[1][2] - wL[2][2] * pos2dL[1];

    matP->m[2*3+0] = matPt->m[0*4+2] = wR[0][0] - wR[2][0] * pos2dR[0];
    matP->m[2*3+1] = matPt->m[1*4+2] = wR[0][1] - wR[2][1] * pos2dR[0];
    matP->m[2*3+2] = matPt->m[2*4+2] = wR[0][2] - wR[2][2] * pos2dR[0];
    matP->m[3*3+0] = matPt->m[0*4+3] = wR[1][0] - wR[2][0] * pos2dR[1];
    matP->m[3*3+1] = matPt->m[1*4+3] = wR[1][1] - wR[2][1] * pos2dR[1];
    matP->m[3*3+2] = matPt->m[2*4+3] = wR[1][2] - wR[2][2] * pos2dR[1];
                                                                                                   
    matQ->m[0] = wL[2][3] * pos2dL[0] - wL[0][3];
    matQ->m[1] = wL[2][3] * pos2dL[1] - wL[1][3];
    matQ->m[2] = wR[2][3] * pos2dR[0] - wR[0][3];
    matQ->m[3] = wR[2][3] * pos2dR[1] - wR[1][3];

    arMatrixMul( matR, matPt, matP );
    arMatrixMul( matS, matPt, matQ );
    if( arMatrixSelfInv( matR ) < 0 ) {
        arMatrixFree( matP );
        arMatrixFree( matPt);
        arMatrixFree( matQ );
        arMatrixFree( matR );
        arMatrixFree( matS );
        arMatrixFree( matT );
        return -1;
    }
    arMatrixMul( matT, matR, matS );

    pos3d[0] = matT->m[0];
    pos3d[1] = matT->m[1];
    pos3d[2] = matT->m[2];
                                                                                                   
    arMatrixFree( matP );
    arMatrixFree( matPt);
    arMatrixFree( matQ );
    arMatrixFree( matR );
    arMatrixFree( matS );
    arMatrixFree( matT );
                                                                                                   
    return 0;
}
