/*******************************************************
 *
 * Author: Hirokazu Kato
 *
 *         kato@sys.im.hiroshima-cu.ac.jp
 *
 * Revision: 2.11
 * Date: 00/05/06
 *
*******************************************************/

#include <stdio.h>
#include <math.h>
#include <AR/ar.h>
#include <AR/matrix.h>

static int    pattern_num = -1;
static int    patf[AR_PATT_NUM_MAX] = { 0 };
static int    pat[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
static double patpow[AR_PATT_NUM_MAX][4];
static int    patBW[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
static double patpowBW[AR_PATT_NUM_MAX][4];

static void   get_cpara( double world[4][2], double vertex[4][2],
                         double para[3][3] );
static int    pattern_match( ARUint8 *data, int *code, int *dir, double *cf );
static void   put_zero( ARUint8 *p, int size );


int arLoadPatt( char *filename )
{
    FILE    *fp;
    int     patno;
    int     h, i, j, l, m;
    int     i1, i2, i3;

    if(pattern_num == -1 ) {
        for( i = 0; i < AR_PATT_NUM_MAX; i++ ) patf[i] = 0;
        pattern_num = 0;
    }

    for( i = 0; i < AR_PATT_NUM_MAX; i++ ) {
        if(patf[i] == 0) break;
    }
    if( i == AR_PATT_NUM_MAX ) return -1;
    patno = i;

    if( (fp=fopen(filename, "r")) == NULL ) {
        printf("\"%s\" not found!!\n", filename);
        return(-1);
    }

    for( h=0; h<4; h++ ) {
        l = 0;
        for( i3 = 0; i3 < 3; i3++ ) {
            for( i2 = 0; i2 < AR_PATT_SIZE_Y; i2++ ) {
                for( i1 = 0; i1 < AR_PATT_SIZE_X; i1++ ) {
                    if( fscanf(fp, "%d", &j) != 1 ) {
                        printf("Pattern Data read error!!\n");
                        return -1;
                    }
                    j = 255-j;
                    pat[patno][h][(i2*AR_PATT_SIZE_X+i1)*3+i3] = j;
                    if( i3 == 0 ) patBW[patno][h][i2*AR_PATT_SIZE_X+i1]  = j;
                    else          patBW[patno][h][i2*AR_PATT_SIZE_X+i1] += j;
                    if( i3 == 2 ) patBW[patno][h][i2*AR_PATT_SIZE_X+i1] /= 3;
                    l += j;
                }
            }
        }
        l /= (AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3);

        m = 0;
        for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) {
            pat[patno][h][i] -= l;
            m += (pat[patno][h][i]*pat[patno][h][i]);
        }
        patpow[patno][h] = sqrt((double)m);
        if( patpow[patno][h] == 0.0 ) patpow[patno][h] = 0.0000001;

        m = 0;
        for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X; i++ ) {
            patBW[patno][h][i] -= l;
            m += (patBW[patno][h][i]*patBW[patno][h][i]);
        }
        patpowBW[patno][h] = sqrt((double)m);
        if( patpowBW[patno][h] == 0.0 ) patpowBW[patno][h] = 0.0000001;
    }
    fclose(fp);

    patf[patno] = 1;
    pattern_num++;

    return( patno );
}

int arFreePatt( int patno )
{
    if( patf[patno] == 0 ) return -1;

    patf[patno] = 0;
    pattern_num--;

    return 1;
}

int arActivatePatt( int patno )
{
    if( patf[patno] == 0 ) return -1;

    patf[patno] = 1;

    return 1;
}

int arDeactivatePatt( int patno )
{
    if( patf[patno] == 0 ) return -1;

    patf[patno] = 2;

    return 1;
}

int arGetCode( ARUint8 *image, int *x_coord, int *y_coord, int *vertex,
               int *code, int *dir, double *cf )
{
    ARUint8 ext_pat[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];

    arGetPatt(image, x_coord, y_coord, vertex, ext_pat);

    pattern_match((ARUint8 *)ext_pat, code, dir, cf);

    return(0);
}

int arGetPatt( ARUint8 *image, int *x_coord, int *y_coord, int *vertex,
               ARUint8 ext_pat[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3] )
{
    double  world[4][2];
    double  local[4][2];
    double  para[3][3];
    double  d, xw, yw;
    int     xc, yc;
    int     i, j;
    int     k1, k2, k3;

    world[0][0] = 100.0;
    world[0][1] = 100.0;
    world[1][0] = 100.0 + 10.0;
    world[1][1] = 100.0;
    world[2][0] = 100.0 + 10.0;
    world[2][1] = 100.0 + 10.0;
    world[3][0] = 100.0;
    world[3][1] = 100.0 + 10.0;
    for( i = 0; i < 4; i++ ) {
        local[i][0] = x_coord[vertex[i]];
        local[i][1] = y_coord[vertex[i]];
    }
    get_cpara( world, local, para );

    put_zero( (ARUint8 *)ext_pat, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 );
    for( j = 0; j < AR_PATT_SAMPLE_NUM; j++ ) {
        yw = 102.5 + 5.0 * (j+0.5) / (double)AR_PATT_SAMPLE_NUM;
        for( i = 0; i < AR_PATT_SAMPLE_NUM; i++ ) {
            xw = 102.5 + 5.0 * (i+0.5) / (double)AR_PATT_SAMPLE_NUM;
            d = para[2][0]*xw + para[2][1]*yw + para[2][2];
            if( d == 0 ) return(-1);
            xc = (int)((para[0][0]*xw + para[0][1]*yw + para[0][2])/d);
            yc = (int)((para[1][0]*xw + para[1][1]*yw + para[1][2])/d);
            if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
                xc = ((xc+1)/2)*2;
                yc = ((yc+1)/2)*2;
            }
            if( xc >= 0 && xc < arImXsize && yc >= 0 && yc < arImYsize ) {
#ifdef AR_PIX_FORMAT_ABGR
                k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+1];
                k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
                   + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
                k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+2];
                k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
                   + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
                k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+3];
                k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
                   + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
#endif
#ifdef AR_PIX_FORMAT_BGRA
                k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+0];
                k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
                   + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
                k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+1];
                k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
                   + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
                k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+2];
                k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
                   + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
#endif
#ifdef AR_PIX_FORMAT_BGR
                k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+0];
                k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
                   + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
                k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+1];
                k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
                   + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
                k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+2];
                k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
                   + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
#endif
#ifdef AR_PIX_FORMAT_RGBA
                k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+2];
                k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
                   + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
                k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+1];
                k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
                   + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
                k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+0];
                k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
                   + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
#endif
#ifdef AR_PIX_FORMAT_RGB
                k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+2];
                k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
                   + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
                k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+1];
                k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
                   + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
                k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE+0];
                k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
                   + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
                ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
#endif
            }
        }
    }

    return(0);
}

static void get_cpara( double world[4][2], double vertex[4][2],
                       double para[3][3] )
{
    ARMat   *a, *b, *c;
    int     i;

    a = arMatrixAlloc( 8, 8 );
    b = arMatrixAlloc( 8, 1 );
    c = arMatrixAlloc( 8, 1 );
    for( i = 0; i < 4; i++ ) {
        a->m[i*16+0]  = world[i][0];
        a->m[i*16+1]  = world[i][1];
        a->m[i*16+2]  = 1.0;
        a->m[i*16+3]  = 0.0;
        a->m[i*16+4]  = 0.0;
        a->m[i*16+5]  = 0.0;
        a->m[i*16+6]  = -world[i][0] * vertex[i][0];
        a->m[i*16+7]  = -world[i][1] * vertex[i][0];
        a->m[i*16+8]  = 0.0;
        a->m[i*16+9]  = 0.0;
        a->m[i*16+10] = 0.0;
        a->m[i*16+11] = world[i][0];
        a->m[i*16+12] = world[i][1];
        a->m[i*16+13] = 1.0;
        a->m[i*16+14] = -world[i][0] * vertex[i][1];
        a->m[i*16+15] = -world[i][1] * vertex[i][1];
        b->m[i*2+0] = vertex[i][0];
        b->m[i*2+1] = vertex[i][1];
    }
    arMatrixSelfInv( a );
    arMatrixMul( c, a, b );
    for( i = 0; i < 2; i++ ) {
        para[i][0] = c->m[i*3+0];
        para[i][1] = c->m[i*3+1];
        para[i][2] = c->m[i*3+2];
    }
    para[2][0] = c->m[2*3+0];
    para[2][1] = c->m[2*3+1];
    para[2][2] = 1.0;
    arMatrixFree( a );
    arMatrixFree( b );
    arMatrixFree( c );
}

static int pattern_match( ARUint8 *data, int *code, int *dir, double *cf )
{
    int    input[AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
    int    i, j, k, l;
    int    ave, sum, res, res2;
    double datapow, sum2, max;

    sum = ave = 0;
    for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) {
        ave += (255-data[i]);
    }
    ave /= (AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3);

    if( arTemplateMatchingMode == AR_TEMPLATE_MATCHING_COLOR ) {
        for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) {
            input[i] = (255-data[i]) - ave;
            sum += input[i]*input[i];
        }
    }
    else {
        for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X;i++) {
            input[i] = ((255-data[i*3+0]) + (255-data[i*3+1]) + (255-data[i*3+02]))/3 - ave;
            sum += input[i]*input[i];
        }
    }

    datapow = sqrt( (double)sum );
    if( datapow == 0.0 ) {
        *code = 0;
        *dir  = 0;
        *cf   = -1.0;
        return -1;
    }

    res = res2 = -1;
    max = 0.0;
    k = -1;
    if( arTemplateMatchingMode == AR_TEMPLATE_MATCHING_COLOR ) {
        for( l = 0; l < pattern_num; l++ ) {
            k++;
            while( patf[k] == 0 ) k++;
            if( patf[k] == 2 ) continue;
            for( j = 0; j < 4; j++ ) {
                sum = 0;
                for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) sum += input[i]*pat[k][j][i];
                sum2 = sum / patpow[k][j] / datapow;
                if( sum2 > max ) { max = sum2; res = j; res2 = k; }
            }
        }
    }
    else {
        for( l = 0; l < pattern_num; l++ ) {
            k++;
            while( patf[k] == 0 ) k++;
            if( patf[k] == 2 ) continue;
            for( j = 0; j < 4; j++ ) {
                sum = 0;
                for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X;i++) sum += input[i]*patBW[k][j][i];
                sum2 = sum / patpowBW[k][j] / datapow;
                if( sum2 > max ) { max = sum2; res = j; res2 = k; }
            }
        }
    }

    *code = res2;
    *dir  = res;
    *cf   = max;

    return 0;
}

static void   put_zero( ARUint8 *p, int size )
{
    while( (size--) > 0 ) *(p++) = 0;
}
