#include <stdio.h>
#include <numeric.h>

#define NUM_FEATURES  10
#define RANGE         100.00

#define MAX_CAMERAS   100 
#define MAX_POINTS    3000

array double camera0[3][4];
array double allthreedpoints[MAX_POINTS][3];
array double twodpoints0[MAX_POINTS][2];

/* take the projection matrix and decompose to fine the rotation, translation and calibration.
   taken from 6.3.2 of Trucco and Verri */
void decomposeprojectionmatrix(array double P[3][4], array double r[3][3], array double t[3], array double K[3][3])
{
    double scale, r31, r32, r33;
    double ox, oy, fx, fy;
    int i,j;
    
    /* destroy the original values */
    for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++) {
            r[i][j] = 0; K[i][j] = 0;
        }
    for (i = 0; i < 3; i++) t[i] = 0;
        
    printf("Proj Approach Extracted R\n%f\n", r);
    printf("Proj Approach Extracted T\n%f\n", t);
    printf("Proj Approach Extracted K\n%f\n", K);

}

/* take the R, T, and K and multiply them together to create the projection matrix P */
void composeprojectionmatrix(array double r[3][3], array double t[3], array double K[3][3], array double P[3][4])
{
 
}

/* compute projection matrix from the three given Euler angles */
void eulerangletorotmatrix(double a, double b, double c, double array r[3][3])
{
    array double r1[3][3], r2[3][3], r3[3][3];
    
    r1[0][0] = cos(a); r1[0][1] = -sin(a);r1[0][2] = 0;
    r1[1][0] = sin(a); r1[1][1] = cos(a); r1[1][2] = 0;
    r1[2][0] = 0;      r1[2][1] = 0;      r1[2][2] = 1.0;
    
    r2[0][0] = cos(b); r2[0][1] = 0;      r2[0][2] = sin(b);
    r2[1][0] = 0;      r2[1][1] = 1.0;    r2[1][2] = 0;
    r2[2][0] = -sin(b);r2[2][1] = 0;      r2[2][2] = cos(b);
    
    r3[0][0] = 1.0;    r3[0][1] = 0;      r3[0][2] = 0;
    r3[1][0] = 0;      r3[1][1] = cos(c); r3[1][2] = -sin(c);
    r3[2][0] = 0;      r3[2][1] = sin(c); r3[2][2] = cos(c);
    
    r = r1*r2*r3;
    //printf("r %f\n", r);
    //printf("Rot Mults\n %f %f\n", r*transpose(r), transpose(r)*r);
}


/* just apply the projection matrix to the 3d points to get a set of 2d points */
void applyprojectionmatrix(array double threedpoints[NUM_FEATURES][3], 
                           array double twodprojections[NUM_FEATURES][2],
                           array double projectionmatrix[3][4])
{
 
        
}


/* compute a set of random 3d points, all of which have positive values in x, y and z */
void computerandomthreedpoints(array double threedpoints[NUM_FEATURES][3])
{
    int i;
    for (i = 0; i < NUM_FEATURES; i++) {
        threedpoints[i][0] = RANGE*urand(NULL); 
        threedpoints[i][1] = RANGE*urand(NULL); 
        threedpoints[i][2] = RANGE*urand(NULL);
    }
    //printf("Three d Points\n", threedpoints);
}

/* given a set of correspondences from 3d to 2d compute the aspect ratio,
   which is fx/fy and the translation in x, and y. print out these three values.
   use method described in 6.2.2 in Trucco and Verri */
void computetsaicalibration(array double threedpoints[NUM_FEATURES][3], 
                            array double twodprojections[NUM_FEATURES][2],
                            array double r[3][3], array double t[3], array double K[3][3])
{
    array double A[NUM_FEATURES][8];
    array double At[NUM_FEATURES][8];
    array double evalues[8];
    array double evectors[8][8];
    array double AtA[8][8];
    double smallesteigneval = 1e10; /* large number */
    double X1,Y1,Z1,x,y;
    int smallestindex,i,j;
    double s, r1[3], r2[3], aspect_ratio, tx, ty, tz;


    /* destroy the original values */
    for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++) {
            r[i][j] = 0; K[i][j] = 0;
        }
    for (i = 0; i < 3; i++) t[i] = 0;
        
    printf("Tsai Approach Aspect ratio %f\n", aspect_ratio);
    printf("Tsai Approach tx %f ty %f\n", tx, ty);

}
/* given a set of correspondences from 3d to 2d points compute the projection matrix
   that produces these correspondence values. Uses the method described in 6.3 of Trucco and Verri */
void computeprojectionmatrix(array double threedpoints[NUM_FEATURES][3], 
                             array double twodprojections[NUM_FEATURES][2],
                             array double projectionmatrix[3][4])
{
    array double A[2*NUM_FEATURES][12];
    array double At[2*NUM_FEATURES][12];
    array double m[12];
    array double evalues[12];
    array double evectors[12][12];
    array double AtA[12][12];
    double smallesteigneval = 1e10; /* large number */
    double X1,Y1,Z1,x,y;
    int smallestindex,i;

 
}

/* set the K matrix to some arbitrary values */
void setkmatrix(array double K[3][3])
{
    int i, j;
    for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++)
            K[i][j] = 0;
    K[0][0] = -1000; K[1][1] = -2000;

    K[2][2] = 1.0;
}

/* set translation to some arbitrary value */
void settrans(array double t[3])
{
    t[0] = 10.0; t[1] = 15.0; t[2] = 20.0;
}

int main( int argc, char** argv )
{
    array double threedpoints[NUM_FEATURES][3];
    array double twodprojections[NUM_FEATURES][2];
    array double projectionmatrix[3][4];
    array double r[3][3];
    array double t[3], K[3][3];
    int i, j;

    eulerangletorotmatrix(.2, .4, .6, r);
    setkmatrix(K);
    settrans(t);
    printf("\nOriginal R\n%f\n", r);
    printf("Original T\n%f\n", t);
    printf("Original K\n%f\n", K);
    composeprojectionmatrix(r, t, K, projectionmatrix);
    
    /* first compute some random 3d points */
    computerandomthreedpoints(threedpoints);
    /* apply the projection matrix to the 3d points */
    applyprojectionmatrix(threedpoints, twodprojections, projectionmatrix); 
    /* now compute the calibration from the 3d and 2d points using the TSAI approach */
    computetsaicalibration(threedpoints, twodprojections, r, t, K);
    /* now compute a new projection matrix only from the 3d and 2d points */
    computeprojectionmatrix(threedpoints, twodprojections, projectionmatrix);
    /* decompose projection matrix to get extrinsic and intrsinic calibration */
    decomposeprojectionmatrix(projectionmatrix, r, t, K);
    /* the R, T and K matrix should be same as was set orignally since data has no noise */


}

 
/* give students eulerangletorotmatrix, setkmatrix, settrans,computerandomthreedpoints.
   they must write the code for all the other routines in the main program. */