/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                         License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2009, PhaseSpace Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The names of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "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 the Intel Corporation or contributors 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.
//
//M*/

#include "cvtest.h"

#if 0

#if defined WIN32 || defined _WIN32
#include "direct.h"
#else
#include <sys/stat.h>
#endif

using namespace cv;

//global variable
#ifndef MAX_PATH
#define MAX_PATH 1024
#endif

char g_filepath[MAX_PATH];

//objects for virtual environmnt

/*****************************Camera class ************************************************/
class Camera
{
public:
    Camera(void);
    virtual ~Camera(void);

    inline void SetPosition(double x, double y, double z)
    {
        position[0] = x;
        position[1] = y;
        position[2] = z;
    }    

    inline double* GetPosition() { return position; }

    inline void SetDistortion(double r1, double r2, double t1, double t2)
    {
        distortion[0] = r1;
        distortion[1] = r2;
        distortion[2] = t1;
        distortion[3] = t2;             
    }      
    inline void SetIntrinsics( double fx, double fy, double cx, double cy ) //fx,fy,cx,cy
    {
        intrinsic[0] =    fx;
        intrinsic[1] =    0;
        intrinsic[2] =    cx;
        intrinsic[3] =    0;
        intrinsic[4] =    fy;
        intrinsic[5] =    cy;
        intrinsic[6] =    0;
        intrinsic[7] =    0;
        intrinsic[8] =    1.0;  
    }

    CvPoint3D64f ConvertPoint2WCS( CvPoint3D64f pt )
    {
        double tmp[3];
        CvMat tmp_point = cvMat( 3, 1, CV_64FC1, tmp );
                
        CvMat rot = cvMat( 3, 3, CV_64FC1, rotation );
        CvMat trans = cvMat( 3, 1, CV_64FC1, translation );
        CvMat in_point = cvMat( 3, 1, CV_64FC1, &pt );

        // out = inv(R) * (in - t)

        cvSub(&in_point, &trans, &tmp_point);
        cvGEMM( &rot, &tmp_point, 1, NULL, 0, &tmp_point, CV_GEMM_A_T /* use transposed rotmat*/  );
        
        CvPoint3D64f out;
        out.x = tmp[0];
        out.y = tmp[1];
        out.z = tmp[2]; 

        return out;
    }

    inline void SetRotation( double* rotmat ) //fx,fy,cx,cy
    {
        memcpy( rotation, rotmat, 9 * sizeof(double) );  
    }        

    double* GetRotation()
    {
        return rotation;
    }

    void ComputeTranslation()
    {
        //convert camera position in WCS into translation vector of the camera 
        //t = -R*pos
        translation[0] = -(rotation[0]*position[0]+rotation[1]*position[1]+rotation[2]*position[2]);
        translation[1] = -(rotation[3]*position[0]+rotation[4]*position[1]+rotation[5]*position[2]);
        translation[2] = -(rotation[6]*position[0]+rotation[7]*position[1]+rotation[8]*position[2]);  
    }

    double* GetTranslation()
    {   
        return translation;
    }
    double* GetIntrinsics()
    {
        return intrinsic;
    }               

    double* GetDistortion()
    {
        return distortion;
    }

    void SetResolution(CvSize res)
    {
        resolution = res;
    }
    CvSize GetResolution()
    {
        return resolution;
    } 
    void saveCamParams(FILE* stream);
    void readCamParams(FILE* stream);
                

protected:
    double distortion[4]; //distortion coeffs according to OpenCV (2 radial and 2 tangential)
    double intrinsic[9];     //matrix of intrinsic parameters
    double translation[3]; //camera's translation vector 
    double rotation[9]; //camera rotation matrix (probably need to convert to camera axis vector
    double position[3]; //camera's position in WCS  

    CvSize resolution;   
};

Camera::Camera(void)
{
    //default parameters
    translation[0] = translation[1] = translation[2] = 0.0;
    rotation[0] = rotation[1] = rotation[2] =
    rotation[3] = rotation[4] = rotation[5] =
    rotation[6] = rotation[7] = rotation[8] = 0.0;
    rotation[0] = rotation[4] = rotation[8] =  1.0;

    distortion[0] = distortion[1] = distortion[2] = distortion[3] = 0.0;
}

Camera::~Camera(void)
{
}

void Camera::saveCamParams(FILE * stream)
{   
    float tmp0 = 0.0f, tmp1 = 1.0f;
    
    // printing camera distortion. 4 parameters
    fprintf(stream, "%.12f %.12f %.12f %.12f\n", distortion[0], distortion[1],
                                                 distortion[2], distortion[3]);
    
    //printing camera intrinsics matrix
    fprintf(stream, "%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n", 
        intrinsic[0], intrinsic[1], intrinsic[2],
        intrinsic[3], intrinsic[4], intrinsic[5],
        intrinsic[6], intrinsic[7], intrinsic[8]);
        
    //printing camera extrinsic transform
    fprintf(stream, "%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n",
        rotation[0], rotation[1], rotation[2], translation[0],
        rotation[3], rotation[4], rotation[5], translation[1],
        rotation[6], rotation[7], rotation[8], translation[2],
        tmp0, tmp0, tmp0, tmp1);           
}

void Camera::readCamParams(FILE* stream)
{   
    double dummy;
    
    // read camera distortion. 4 parameters
    fscanf(stream, "%lf %lf %lf %lf\n", &(distortion[0]), &(distortion[1]), &(distortion[2]), &(distortion[3]) );
    
    //read camera intrinsics matrix
    fscanf(stream, "%lf %lf %lf %lf %lf %lf %lf %lf %lf\n", 
        &(intrinsic[0]), &(intrinsic[1]), &(intrinsic[2]),
        &(intrinsic[3]), &(intrinsic[4]), &(intrinsic[5]),
        &(intrinsic[6]), &(intrinsic[7]), &(intrinsic[8]));
        
    //read camera extrinsic transform
    fscanf(stream, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
        &(rotation[0]), &(rotation[1]), &(rotation[2]), &(translation[0]),
        &(rotation[3]), &(rotation[4]), &(rotation[5]), &(translation[1]),
        &(rotation[6]), &(rotation[7]), &(rotation[8]), &(translation[2]),
        &dummy, &dummy, &dummy, &dummy);   
}

/******************************** body**************************************************/

class RigidBody
{
    //global position of body's center
    double pos[3];
    double rotation[9]; //body's rotation matrix 
    
    int* m_ids; //points ids, these are integer numbers not necessarily zero-based indices
    CvPoint3D64f* m_points3D;  //these are points in object coordinate system
    CvPoint3D64f* m_points3D_WCS;  //these are points in WCS
    int m_numPoints;

public:

    RigidBody()
    {
        m_ids = 0;
        m_points3D = 0;
        m_points3D_WCS = 0;
        m_numPoints = 0;

        SetPosition(0,0,0);
        double t[9] = {1,0,0,0,1,0,0,0,1};
        SetRotation( t );
    }
    virtual ~RigidBody() {}

    virtual void SetPosition(double x, double y, double z )
    {
        pos[0] = x;
        pos[1] = y;
        pos[2] = z;
    }
    
    virtual void SetRotation( double* rotmat ) 
    {
        memcpy( rotation, rotmat, 9 * sizeof(double) );     
    }    

    void Save(char* fname, bool wcs /* 1 - save coordinates in wcs, 0 - save coordinates in object coordinate system*/  )
    {
        FILE* fp = fopen(fname, "w");
        fprintf(fp, "%d\n", m_numPoints );
        if( wcs )
        {
            for( int i = 0; i < m_numPoints; i++ )
            {   
                fprintf(fp, "%d %.12f %.12f %.12f\n", m_ids[i], m_points3D_WCS[i].x, m_points3D_WCS[i].y, m_points3D_WCS[i].z ); 
            }
        }
        else
        {
            for( int i = 0; i < m_numPoints; i++ )
            {   
                fprintf(fp, "%d, %.12f, %.12f, %.12f\n", m_ids[i], m_points3D[i].x, m_points3D[i].y, m_points3D[i].z ); 
            }
        }   
        fclose(fp);
    }

    void Load(char* fname)
    {   
        clear_points();
        FILE* fp = fopen(fname, "r");
        fscanf(fp, "%d\n", &m_numPoints );
        //allocate arrays
        m_points3D = new CvPoint3D64f[m_numPoints];
        m_points3D_WCS = new CvPoint3D64f[m_numPoints];
        m_ids = new int[m_numPoints];
        
        for(int i = 0; i < m_numPoints; i++ )
        {
            fscanf(fp, "%d %lf %lf %lf\n", &(m_ids[i]), &(m_points3D[i].x), &(m_points3D[i].y), &(m_points3D[i].z));
        } 
    }

    void clear_points()
    {   
        if(m_points3D)
            delete m_points3D;
        m_points3D = NULL;
        if(m_points3D_WCS)
            delete m_points3D_WCS;
        m_points3D_WCS = NULL;
        if( m_ids )
            delete m_ids;
        m_ids = NULL;
        m_numPoints = 0;
    }

    void generate_random( int n, double x_min, double x_max, 
                                 double y_min, double y_max,
                                 double z_min, double z_max )
    {
        clear_points();
        m_numPoints = n;
        m_points3D = new CvPoint3D64f[m_numPoints];
        m_points3D_WCS = new CvPoint3D64f[m_numPoints];
        m_ids = new int[m_numPoints];

        //fill ids
        for( int i = 0 ; i < n; i++ )
        {
            m_ids[i] = i;
        }    

        CvRNG rng;

        CvMat values = cvMat( m_numPoints, 1, CV_64FC3, m_points3D  );
        
        cvRandArr( &rng, &values, CV_RAND_UNI,
                   cvScalar(x_min, y_min, z_min), // min 
                   cvScalar(x_max, y_max, z_max) // deviation
                 ); 
    } 
    CvPoint3D64f* GetPoints3D(bool wcs)
    {    
        if( wcs )
        {
            //fill points in WCS accordingly to rotation matrix and board position
            for(int i = 0; i < NumPoints(); i++ )
            {                
                m_points3D_WCS[i].x = rotation[0]*m_points3D[i].x + 
                                      rotation[1]*m_points3D[i].y + 
                                      rotation[2]*m_points3D[i].z + pos[0];

                m_points3D_WCS[i].y = rotation[3]*m_points3D[i].x + 
                                  rotation[4]*m_points3D[i].y + 
                                  rotation[5]*m_points3D[i].z + pos[1];

                m_points3D_WCS[i].z = rotation[6]*m_points3D[i].x + 
                                  rotation[7]*m_points3D[i].y + 
                                  rotation[8]*m_points3D[i].z + pos[2];
            }  
            //return points in global cooordinates
            return m_points3D_WCS;

        }
        else
            return m_points3D;
    } 
        
    double* GetRotation() { return rotation; } 

    int NumPoints()
    {
        return m_numPoints;
    }

    int* GetPointsIds()
    {
        return this->m_ids;
    }

};


/********************************* environment *****************************************/
#define PT_INVISIBLE 0
#define PT_VISIBLE 1
#define PT_OUTBORDER 2

class Environment
{
public:
    Environment(void);
    virtual ~Environment(void);

    inline int NumCameras()
    {
        return (int)m_cameras.size();
    }

    inline int AddCamera(Camera* cam)
    {
        m_cameras.push_back(cam);
        return (int)m_cameras.size();
    }    
    
    RigidBody* GetBody()
    {
        return m_body;
    }

    void SetNoise(float n) 
    { 
        noise = n; 
    }

    int Capture();
    int Save(char* filename);
    
protected:

    std::vector<Camera*> m_cameras;
    RigidBody* m_body;
    std::vector<CvPoint2D64f*> m_image_points;
    std::vector<int*> m_point_visibility;
    float noise;

};

Environment::Environment(void)
{       
    m_body = new RigidBody();
}

Environment::~Environment(void)
{
    delete m_body;
}

int Environment::Capture()
{
    //clear points
    for( size_t i = 0; i < m_image_points.size(); i++ )
    {
        if( m_image_points[i]  )
            delete m_image_points[i]; 
    }
    m_image_points.clear();
    
    for( size_t i = 0; i < m_point_visibility.size(); i++ )
    {
        if( m_point_visibility[i]  )
            delete m_point_visibility[i]; 
    }
    m_point_visibility.clear();            
    
    CvPoint3D64f* board_pts = m_body->GetPoints3D(true);
    int num_points = m_body->NumPoints();        

    //loop over cameras 
    for( size_t i = 0; i < m_cameras.size(); i++ )
    {
        //get camera parameters       
        //project points onto camera image 
        
        double* rot = m_cameras[i]->GetRotation();
        double* trans = m_cameras[i]->GetTranslation();
        double* intr = m_cameras[i]->GetIntrinsics();
        double* dist = m_cameras[i]->GetDistortion();

        CvPoint2D64f* image_points = new CvPoint2D64f[num_points];
        m_image_points.push_back(image_points);

        int* points_visibility = new int[num_points];
        m_point_visibility.push_back(points_visibility);
                        
        cvProjectPointsSimple(num_points, board_pts, rot, trans, intr, dist, image_points);    

        CvRNG rng;

        if( noise > 0)
        {
            CvMat* values = cvCreateMat( num_points, 1, CV_32FC2 );

            float stdev = noise;         

            cvRandArr( &rng, values, CV_RAND_NORMAL,
                       cvScalar(0.0, 0.0), // mean
                       cvScalar(stdev, stdev) // deviation
                       );      

            //add gaussian noise to image points
            
            for( int j = 0; j < num_points; j++ )
            {
                CvPoint2D32f pt = *(CvPoint2D32f*)cvPtr1D( values, j, 0 );

                pt.x = min( pt.x, stdev);
                pt.x = max( pt.x, -stdev);
                
                pt.y = min( pt.y, stdev);
                pt.y = max( pt.y, -stdev);   
                                
                image_points[j].x += pt.x;
                image_points[j].y += pt.y;
            }                
            cvReleaseMat( &values );            
        }

        //decide if point visible to camera
        //loop over points and assign visibility flag to them
        for( int j = 0; j < num_points; j++ )
        {
            //generate random visibility of the point
            int visible = cvRandInt(&rng) % 2; //visibility 50% 
            
            //check the point is in camera FOV    (1-pixel border assumed invisible)
            if( image_points[j].x > 0 && image_points[j].x < m_cameras[i]->GetResolution().width-1 
                && image_points[j].y > 0 && image_points[j].y < m_cameras[i]->GetResolution().height-1 )
            {
                if(!visible)
                    points_visibility[j] = PT_INVISIBLE;
                else
                    points_visibility[j] = PT_VISIBLE;
            }
            else
                points_visibility[j] = PT_OUTBORDER;             
        }  
    }  

    //some points may become completely invisible for all cameras or visible only at one camera
    //we will forcely make them visible for at least 2 cameras
    for( int i = 0 ; i < num_points; i++ )
    {
        int numvis = 0;
        for( size_t j = 0; j < m_cameras.size(); j++ )
        {
            if( m_point_visibility[j][i] == PT_VISIBLE )
                numvis++;
        }

        if(numvis < 2)
        {
            for( size_t j = 0; j < m_cameras.size(); j++ )
            {
                if( m_point_visibility[j][i] == PT_INVISIBLE )
                {
                    m_point_visibility[j][i] = PT_VISIBLE;
                    numvis++;
                }
                if(numvis > 1)
                    break;
            }  
            assert(numvis > 1 );
        }    
    }

    return 0;
}

/*void Environment::TestCamera(int camind)
{
    CvPoint3D64f* board_pts = m_body->GetPoints3D(false);

    int count = 0;
    for( int j = 0; j < m_body->NumPoints(); j++ )
    {
        if( m_point_visibility[camind][j] == PT_VISIBLE )
            count++;
    }

    CvMat* object_points = cvCreateMat( 1, count, CV_64FC3 );
    CvMat* image_points  = cvCreateMat( 1, count, CV_64FC2 );
    CvMat* intrinsic_matrix  = cvCreateMatHeader( 3, 3, CV_64FC1 );
       cvSetData(intrinsic_matrix, m_cameras[camind]->GetIntrinsics(), 3*sizeof(double) );

    CvMat* distortion_coeffs  = cvCreateMat( 4, 1, CV_64FC1 );   cvSetZero(distortion_coeffs);
    CvMat* rotation_vector = cvCreateMat( 3, 1, CV_64FC1 );
    CvMat* translation_vector = cvCreateMat( 3, 1, CV_64FC1 );

    CvMat* rotation_matrix = cvCreateMat( 3, 3, CV_64FC1 );

    //loop over points and assign visibility flag to them
    int ind = 0;
    for( int j = 0; j < m_body->NumPoints(); j++ )
    {
        if( m_point_visibility[camind][j] == PT_VISIBLE)
        {
            //add to matrix
            *((CvPoint3D64f*)(object_points->data.db + ind*3)) = board_pts[j];
            *((CvPoint2D64f*)(image_points->data.db + ind*2)) = m_image_points[camind][j];
            ind++;
        }             
    }  
    

    //find extrinsic parameters of the board 
    cvFindExtrinsicCameraParams2( object_points,
                                  image_points,
                                  intrinsic_matrix,
                                  distortion_coeffs,
                                  rotation_vector,
                                  translation_vector );

    //cvRodrigues2( rotation_vector, rotation_matrix );

    
    //reproject points and see error of reprojection

    CvMat* image_points_repro = cvCloneMat( image_points );  

    cvProjectPoints2( object_points, rotation_vector, translation_vector, intrinsic_matrix, distortion_coeffs,
                          image_points_repro );

    for( int j = 0; j < image_points_repro->cols; j++ )
    {
        CvPoint2D64f pt_orig = *((CvPoint2D64f*)(image_points->data.db + j*2));
        CvPoint2D64f pt_repro = *((CvPoint2D64f*)(image_points_repro->data.db + j*2));

        CvPoint2D64f diff;
        diff.x = pt_repro.x - pt_orig.x;
        diff.y = pt_repro.y - pt_orig.y;    

    }   
}     */

int Environment::Save(char* filename)
{               
    int* ind = m_body->GetPointsIds();
    FILE* saveFile = fopen( filename, "w" );
    if(saveFile) 
    {
        for( size_t cam_index = 0; cam_index < m_cameras.size(); cam_index++ )
        {     
            CvPoint2D64f* image_points = m_image_points[cam_index];

            int numPnt = 0; //no visible points by default

            //check points visibility             
            //camera is visible, check individual points 
            for(int i = 0; i < m_body->NumPoints(); i++ )
            {
                if( m_point_visibility[cam_index][i] == PT_VISIBLE)
                {
                    //point is visible 
                    numPnt++;
                }                         
            }    
            
            if(numPnt)    //some points are visible
            {
                for( int i = 0; i < m_body->NumPoints(); i++) 
                {
                    if( m_point_visibility[cam_index][i] == PT_VISIBLE )
                    {
                        //point is visible 
                        fprintf(saveFile, "%d %d %d %.12f %.12f\n", 0 /*snapshot_id*/, (int)cam_index, ind[i], image_points[i].x, image_points[i].y ); 
                    }
                }
            }

        }
    }

    //close file 
    fclose(saveFile);
    return 0;
}



  
//input parameters for envirinment generation
struct Params
{
    int width; //width of images
    int height; //height of images
    float FOVx; //camera field of view in horizontal direction. vertical FOV is detected from aspect ratio
    float noise; //corners projection noise
    float k1, k2, k3; //radial distortion coeffs
    float p1,p2; //tangential distortion coeffs
};

int GenerateTestData2(Params& params)
{
    //create environment
    Environment* env = new Environment();
    env->SetNoise(params.noise);

    CvSize im_res = cvSize(params.width,params.height);

    double FOVx = params.FOVx; // field of view relatively to width  (in degrees)
    double fov_rad = FOVx * CV_PI / 180;
    double fx = im_res.width/2.0 / tan( fov_rad/2);
    double fy = fx;
    double cx = im_res.width/2.0-0.5;
    double cy = im_res.height/2.0-0.5;

    //model cube room of size 2x2x2 meters
    //8 cameras on the perimeter at the height 1 meter in corners and in the middle of walls
    // all they look to the center of the room
    //coordinate center in the center of the cube
    //Z coordinate is oriented up
        
    Camera* cam[8];
    for( int i = 0; i < 8; i++ )
    {
        cam[i] = new Camera();
        cam[i]->SetDistortion(params.k1, params.k2,0,0);  //only radial distortion
        cam[i]->SetIntrinsics(fx,fy, cx, cy ); //fx,fy,cx,cy
        cam[i]->SetResolution(im_res);  
        env->AddCamera(cam[i]);
    }

    //set positions
    cam[0]->SetPosition( 1.,1.,0);
    cam[1]->SetPosition( 0.,1.,0);
    cam[2]->SetPosition( -1.,1.,0);
    cam[3]->SetPosition( -1.,0.,0);
    cam[4]->SetPosition( -1.,-1.,0);
    cam[5]->SetPosition( 0.,-1.,0);
    cam[6]->SetPosition( 1.,-1.,0);
    cam[7]->SetPosition( 1.,0.,0);

    //set rotation matrices
    //they will be oriented strongly vertically and rotated only around vertical axis (Z coorinate in WCS)
        
    CvMat* camrot = cvCreateMat( 3, 3, CV_64FC1);

    for( int i = 0; i < 8; i++ )
    {
        //y of the camera is oriented along negative Z of WCS
        double yDirCamera[3] = {0,0,-1};
        double zDirCamera[3]; //oriented from camera center to WCS center
        double* pos = cam[i]->GetPosition();
        zDirCamera[0] = -pos[0];
        zDirCamera[1] = -pos[1];
        zDirCamera[2] = -pos[2]; 

        double xDirCamera[3]; //cross product Y*Z
        xDirCamera[0] = yDirCamera[1]*zDirCamera[2] - yDirCamera[2]*zDirCamera[1];
        xDirCamera[1] = yDirCamera[2]*zDirCamera[0] - yDirCamera[0]*zDirCamera[2];
        xDirCamera[2] = yDirCamera[0]*zDirCamera[1] - yDirCamera[1]*zDirCamera[0];

        //normalize z and x
        double inv_norm = 1.0/sqrt(xDirCamera[0]*xDirCamera[0] + xDirCamera[1]*xDirCamera[1] + xDirCamera[2]*xDirCamera[2]);
        xDirCamera[0]*=inv_norm;
        xDirCamera[1]*=inv_norm;
        xDirCamera[2]*=inv_norm;

        inv_norm = 1.0 / sqrt(zDirCamera[0]*zDirCamera[0] + zDirCamera[1]*zDirCamera[1] + zDirCamera[2]*zDirCamera[2]);
        zDirCamera[0] *= inv_norm;
        zDirCamera[1] *= inv_norm;
        zDirCamera[2] *= inv_norm; 

        camrot->data.db[0] = xDirCamera[0];
        camrot->data.db[3] = xDirCamera[1];
        camrot->data.db[6] = xDirCamera[2];

        camrot->data.db[1] = yDirCamera[0];
        camrot->data.db[4] = yDirCamera[1];
        camrot->data.db[7] = yDirCamera[2];
        
        camrot->data.db[2] = zDirCamera[0];
        camrot->data.db[5] = zDirCamera[1];
        camrot->data.db[8] = zDirCamera[2];    
        
        //get inverse matrix (equal to transposed)
        cvTranspose(camrot, camrot);

        cam[i]->SetRotation(camrot->data.db);
        cam[i]->ComputeTranslation();// compute translation after we set position and rotation matrix    
    }
 
#if defined WIN32 || defined _WIN32
    _mkdir(g_filepath);
#else
    mkdir(g_filepath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#endif
    char fname[2048];  
    sprintf(fname, "%scameras.txt", g_filepath );
    FILE* clist = fopen(fname, "w");
    for(unsigned int i = 0; i < 8; i++)
    {            
        sprintf(fname, "%scamera%d.calib", g_filepath, i );
        FILE *calibFile = fopen(fname,"w");
        cam[i]->saveCamParams(calibFile);
        fclose( calibFile ); 
        fprintf(clist, "%s\n", fname);
    }                                 
    fclose(clist); 

    std::vector<CvPoint3D64f> board_pos;
    board_pos.clear();

    //generate body position, set zero for now
    CvPoint3D64f pos;
    pos.x = 0; pos.y = 0; pos.z = 0;                 
    board_pos.push_back(pos);
        
    RigidBody* body = env->GetBody();
    body->generate_random(50, -0.25, 0.25, -0.25, 0.25, -0.25, 0.25 );
    
    CvRNG rng = cvRNG();
    int index = 0; //index of board's position to save, is used to enumerate snapshots
    
    for(size_t i = 0; i < board_pos.size(); i++ )
    {
        CvPoint3D64f point = board_pos[i];
        body->SetPosition(point.x,point.y,point.z);

        //generate random orientation of the board 
        unsigned int rn = cvRandInt(&rng);
        //scale to 90 degrees
        rn = rn%90;
        double slant = (double)rn-45-90; //angle of slant of the bord relatively to its horizontal direction
        
        rn = cvRandInt(&rng);
        //scale to 360 degrees
        rn = rn%360;
        double azimuth = rn; //azimuth


        //set slant and azimuth to 0
        slant = azimuth = 0;

        double sn = sin(azimuth*CV_PI/180);
        double cs = cos(azimuth*CV_PI/180);  

        //generate rotation matrix
        CvMat* mat_azimuth = cvCreateMat( 3, 3, CV_64FC1);
        mat_azimuth->data.db[0] = cs;
        mat_azimuth->data.db[1] = -sn;
        mat_azimuth->data.db[2] = 0;

        mat_azimuth->data.db[3] = sn;
        mat_azimuth->data.db[4] = cs;
        mat_azimuth->data.db[5] = 0;
        
        mat_azimuth->data.db[6] = 0;
        mat_azimuth->data.db[7] = 0;
        mat_azimuth->data.db[8] = 1;

        sn = sin(slant*CV_PI/180);
        cs = cos(slant*CV_PI/180);  
        CvMat* mat_slant = cvCreateMat( 3, 3, CV_64FC1);  //rotation around X axis
        mat_slant->data.db[0] = 1;
        mat_slant->data.db[1] = 0;
        mat_slant->data.db[2] = 0;

        mat_slant->data.db[3] = 0;
        mat_slant->data.db[4] = cs;
        mat_slant->data.db[5] = -sn;
        
        mat_slant->data.db[6] = 0;
        mat_slant->data.db[7] = sn;
        mat_slant->data.db[8] = cs;

        CvMat* rot = cvCreateMat( 3, 3, CV_64FC1);  //rotation around X axis
        //create complete rotation matrix
        cvMatMul(mat_azimuth, mat_slant, rot);    

        //these are coordinates of board's axis in WCS
        body->SetRotation(rot->data.db);
        
        //project images onto all cameras
        env->Capture();

        //save 3d points of corners into the file
        char fname[2048];
        sprintf(fname, "%sPoints%d.3d", g_filepath, index );
        body->Save(fname, true);
        //save shots from camera

        sprintf(fname, "%salldata.txt", g_filepath );
        env->Save(fname);
    }      
    //destroy everything
    delete env;   
    return 0;
}    

int TestLevmar2()
{
    //load data from file

    // 1. load ground truth 3D points
    RigidBody body;

    char fname[2048];
    sprintf(fname, "%sPoints0.3d", g_filepath );
    body.Load(fname);
    int num_points = body.NumPoints(); 

    // 2. load cameras
    int num_cams = 0;
    //int num_cam_param = 10;
    std::vector<Camera*> cams;

    sprintf(fname, "%scameras.txt", g_filepath );    
    FILE* fp = fopen(fname, "r" );
    while( !feof(fp) )
    {
        char fname[MAX_PATH];
        fscanf(fp, "%s\n", fname );
        Camera* newcam = new Camera();
        FILE* fp2 = fopen( fname, "r" );
        newcam->readCamParams(fp2);
        cams.push_back(newcam);
        fclose(fp2);
        num_cams++;
    }
    fclose(fp);   

    // 2. Load projection data  
    CvMat* m = cvCreateMat( body.NumPoints(), num_cams, CV_64FC2 );
    //all invisible point will have (-1,-1) projection
    cvSet( m, cvScalar(-1,-1) );

    sprintf(fname, "%salldata.txt", g_filepath );    
    fp = fopen( fname, "r");
    
    int counter = 0;
    while( !feof(fp) )
    {
        //read line
        int snapid, cameraid, pointid;
        double x,y;
        fscanf(fp, "%d %d %d %lf %lf\n", &snapid, &cameraid, &pointid, &x, &y);
        cvSet2D(m, pointid, cameraid, cvScalar(x,y) );         
        counter++;
    }

    vector<vector<int> > visibility;
    
    //transform this matrix to measurement vector
    vector<vector<Point2d> > imagePoints;
    
    for(int j = 0; j < num_cams; j++ )
    {
        vector<Point2d> vec;
        vector<int> visvec;
        for(int i = 0; i < num_points; i++ )
        {          
            CvScalar s = cvGet2D( m, i, j ); 
            if( s.val[0] != -1 ) //point is visible
            {
                vec.push_back(Point2d(s.val[0],s.val[1]));
                visvec.push_back(1);                
            } 
            else
            {
                vec.push_back(Point2d(DBL_MAX,DBL_MAX));            
                visvec.push_back(0);      
            }
        } 
        imagePoints.push_back(vec);
        visibility.push_back(visvec);
    }
        
    //form initial params
    vector<Mat> cameraMatrix; //intrinsic matrices of all cameras (input and output)
    vector<Mat> R; //rotation matrices of all cameras (input and output)
    vector<Mat> T; //translation vector of all cameras (input and output)
    vector<Mat> distCoeffs; //distortion coefficients of all cameras (input and output)
    
    //store original camera positions
    vector<Mat> T_backup;
    //store original camera rotations
    vector<Mat> R_backup;
    //store original intrinsic matrix
    vector<Mat> cameraMatrix_backup;
    //store original distortion
    vector<Mat> distCoeffs_backup;
    
    for( int i = 0; i < (int)cams.size(); i++ )
    {
        //fill params
        Camera* c = cams[i]; 
        //rotation
        double* rotmat = c->GetRotation();
        Mat rm(3,3,CV_64F,rotmat);
        R_backup.push_back(rm);
        //generate small random rotation
        Mat rodr; Rodrigues(rm, rodr);
        double n = norm(rodr);
        //add small angle 
        //add about 5 degrees to rotation 
        rodr *= ((n+0.1)/n);
        Rodrigues(rodr, rm);        
        R.push_back(rm); 
        
        //translation
        double* tr = c->GetTranslation();
        Mat tv(Size(1,3), CV_64F, tr);
        T_backup.push_back(tv);
        //add random translation within 1 cm
        Mat t_(3,1,CV_64F);
        randu(t_, -0.01, 0.01);
        tv+=t_;

        T.push_back(tv);
        //intrinsic matrix
        double* intr = c->GetIntrinsics();
        cameraMatrix_backup.push_back(Mat(Size(3,3), CV_64F, intr));
        cameraMatrix.push_back(Mat(Size(3,3), CV_64F, intr));  

        //distortion         
        Mat d(4, 1, CV_64F, c->GetDistortion() );
        distCoeffs_backup.push_back(d); 

        //variate distortion by 5%
        d*=1.05;
        distCoeffs.push_back(d);
        
    } 
      
    //form input points
    const Point3d* ptptr = (const Point3d*)body.GetPoints3D(true);
    vector<Point3d> points(ptptr, ptptr + num_points);
    //store points
    vector<Point3d> points_backup(num_points);
    std::copy(points.begin(), points.end(), points_backup.begin());
    
    //variate initial points
    CvRNG rng;
    CvMat* values = cvCreateMat( num_points*3, 1, CV_64F );          
    cvRandArr( &rng, values, CV_RAND_NORMAL,
               cvScalar(0.0), // mean
               cvScalar(0.02) // deviation (in meters)
               );  
    CvMat tmp = cvMat(values->rows, values->cols, values->type, &points[0] );
    cvAdd( &tmp, values, &tmp );      
    cvReleaseMat( &values );   
        
    LevMarqSparse::bundleAdjust( points, //positions of points in global coordinate system (input and output)
                  imagePoints, //projections of 3d points for every camera 
                  visibility, //visibility of 3d points for every camera 
                  cameraMatrix, //intrinsic matrices of all cameras (input and output)
                  R, //rotation matrices of all cameras (input and output)
                  T, //translation vector of all cameras (input and output)
                  distCoeffs, //distortion coefficients of all cameras (input and output)
                  TermCriteria(TermCriteria::COUNT|TermCriteria::EPS, 3000, DBL_EPSILON ));
                  //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})

    //compare input points and found points
    double maxdist = 0;
    for( size_t i = 0; i < points.size(); i++ )
    {
        Point3d in = points_backup[i];
        Point3d out = points[i];
        double dist = sqrt(in.dot(out)); 
        if(dist > maxdist)
            maxdist = dist;
    }
    printf("Maximal distance between points: %.12lf\n", maxdist );    

    //compare camera positions
    maxdist = 0;
    for( size_t i = 0; i < T.size(); i++ )
    {
        double dist = norm(T[i], T_backup[i]);
         
        if(dist > maxdist)
            maxdist = dist;         
    }   
    printf("Maximal distance between cameras centers: %.12lf\n", maxdist ); 

    //compare rotation matrices
    maxdist = 0;
    for( size_t i = 0; i < R.size(); i++ )
    {
        double dist = norm(R[i], R_backup[i], NORM_INF);
         
        if(dist > maxdist)
            maxdist = dist;         
    }    
    printf("Maximal difference in rotation matrices elements: %.12lf\n", maxdist ); 

    //compare intrinsic matrices
    maxdist = 0;
    double total_diff = 0;
    for( size_t i = 0; i < cameraMatrix.size(); i++ )
    {
        double fx_ratio = cameraMatrix[i].at<double>(0,0)/
                          cameraMatrix_backup[i].at<double>(0,0);
        double fy_ratio = cameraMatrix[i].at<double>(1,1)/
                          cameraMatrix_backup[i].at<double>(1,1);
        double cx_diff = cameraMatrix[i].at<double>(0,2) -
                          cameraMatrix_backup[i].at<double>(0,2);
        double cy_diff = cameraMatrix[i].at<double>(1,2) -
                          cameraMatrix_backup[i].at<double>(1,2);
        total_diff += fabs(fx_ratio - 1) + fabs(fy_ratio - 1) + fabs(cx_diff) + fabs(cy_diff);
    }
    //ts->printf(CvTS::LOG, "total diff = %g\n", total_diff);

    return 1;
}


class CV_BundleAdjustmentTest : public CvTest
{

public:
    CV_BundleAdjustmentTest();
    ~CV_BundleAdjustmentTest();
    void clear();
    //int write_default_params(CvFileStorage* fs);

protected:
    //int read_params( CvFileStorage* fs );
    int compare(double* val, double* ref_val, int len,
                double eps, const char* param_name);

    void run(int);
};


CV_BundleAdjustmentTest::CV_BundleAdjustmentTest():
    CvTest( "bundleadjust", "bundleAdjust" )
{
    support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
}


CV_BundleAdjustmentTest::~CV_BundleAdjustmentTest()
{
    clear();
}


void CV_BundleAdjustmentTest::clear()
{
    CvTest::clear();
}


int CV_BundleAdjustmentTest::compare(double* val, double* ref_val, int len,
                                      double eps, const char* param_name )
{
    return cvTsCmpEps2_64f( ts, val, ref_val, len, eps, param_name );
}

void CV_BundleAdjustmentTest::run( int start_from )
{
    int code = CvTS::OK;
    char            filepath[100];
    char            filename[100];
    
    CvSize          imageSize;
    int             numImages;
    CvSize          etalonSize;
    
    CvPoint2D64f*   imagePoints;
    CvPoint3D64f*   objectPoints;
    CvPoint2D64f*   reprojectPoints;

    double*         transVects;
    double*         rotMatrs;

    double*         goodTransVects;
    double*         goodRotMatrs;

    double          cameraMatrix[3*3];
    double          distortion[4];

    double          goodDistortion[4];

    int*            numbers;
    FILE*           file = 0;
    FILE*           datafile = 0; 
    int             i,j;
    int             currImage;
    int             currPoint;

    int             calibFlags;
    char            i_dat_file[100];
    int             numPoints;
    int numTests;
    int currTest;

    imagePoints     = 0;
    objectPoints    = 0;
    reprojectPoints = 0;
    numbers         = 0;

    transVects      = 0;
    rotMatrs        = 0;
    goodTransVects  = 0;
    goodRotMatrs    = 0;
    int progress = 0;

    //generate test data
    Params params;
    //set default params
    
    params.FOVx = 30;
    params.height = 480;
    params.width = 640;
    //params.noise = 0.25f;
    params.k1 = -0.5f;
    params.k2 = -0.5f;
    params.k3 = 0.0f;
    params.noise = 0.0f;
    params.p1 = 0;
    params.p2 = 0;  

    sprintf( g_filepath, "%sSBA/", ts->get_data_path() );
   
    GenerateTestData2(params);
    TestLevmar2();
    
    sprintf( filepath, "%scameracalibration/", ts->get_data_path() );
    sprintf( filename, "%sdatafiles.txt", filepath );
    datafile = fopen( filename, "r" );
    if( datafile == 0 ) 
    {
        ts->printf( CvTS::LOG, "Could not open file with list of test files: %s\n", filename );
        code = CvTS::FAIL_MISSING_TEST_DATA;
        goto _exit_;
    }

    fscanf(datafile,"%d",&numTests);

    for( currTest = start_from; currTest < numTests; currTest++ )
    {
        progress = update_progress( progress, currTest, numTests, 0 );
        fscanf(datafile,"%s",i_dat_file);
        sprintf(filename, "%s%s", filepath, i_dat_file);
        file = fopen(filename,"r");

        ts->update_context( this, currTest, true );

        if( file == 0 )
        {
            ts->printf( CvTS::LOG,
                "Can't open current test file: %s\n",filename);
            if( numTests == 1 )
            {
                code = CvTS::FAIL_MISSING_TEST_DATA;
                goto _exit_;
            }
            continue; // if there is more than one test, just skip the test
        }

        fscanf(file,"%d %d\n",&(imageSize.width),&(imageSize.height));
        if( imageSize.width <= 0 || imageSize.height <= 0 )
        {
            ts->printf( CvTS::LOG, "Image size in test file is incorrect\n" );
            code = CvTS::FAIL_INVALID_TEST_DATA;
            goto _exit_;
        }

        /* Read etalon size */
        fscanf(file,"%d %d\n",&(etalonSize.width),&(etalonSize.height));
        if( etalonSize.width <= 0 || etalonSize.height <= 0 )
        {
            ts->printf( CvTS::LOG, "Pattern size in test file is incorrect\n" );
            code = CvTS::FAIL_INVALID_TEST_DATA;
            goto _exit_;
        }

        numPoints = etalonSize.width * etalonSize.height;

        /* Read number of images */
        fscanf(file,"%d\n",&numImages);
        if( numImages <=0 )
        {
            ts->printf( CvTS::LOG, "Number of images in test file is incorrect\n");
            code = CvTS::FAIL_INVALID_TEST_DATA;
            goto _exit_;
        }

        /* Need to allocate memory */
        imagePoints     = (CvPoint2D64f*)cvAlloc( numPoints *
                                                    numImages * sizeof(CvPoint2D64f));
        
        objectPoints    = (CvPoint3D64f*)cvAlloc( numPoints *
                                                    numImages * sizeof(CvPoint3D64f));

        reprojectPoints = (CvPoint2D64f*)cvAlloc( numPoints *
                                                    numImages * sizeof(CvPoint2D64f));

        /* Alloc memory for numbers */
        numbers = (int*)cvAlloc( numImages * sizeof(int));

        /* Fill it by numbers of points of each image*/
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            numbers[currImage] = etalonSize.width * etalonSize.height;
        }

        /* Allocate memory for translate vectors and rotmatrixs*/
        transVects     = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
        rotMatrs       = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));

        goodTransVects = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
        goodRotMatrs   = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));

        /* Read object points */
        i = 0;/* shift for current point */
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            for( currPoint = 0; currPoint < numPoints; currPoint++ )
            {
                double x,y,z;
                fscanf(file,"%lf %lf %lf\n",&x,&y,&z);

                (objectPoints+i)->x = x;
                (objectPoints+i)->y = y;
                (objectPoints+i)->z = z;
                i++;
            }
        }

        /* Read image points */
        i = 0;/* shift for current point */
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            for( currPoint = 0; currPoint < numPoints; currPoint++ )
            {
                double x,y;
                fscanf(file,"%lf %lf\n",&x,&y);

                (imagePoints+i)->x = x;
                (imagePoints+i)->y = y;
                i++;
            }
        }

        /* Read good data computed before */

        /* Focal lengths */
        double goodFcx,goodFcy;
        fscanf(file,"%lf %lf",&goodFcx,&goodFcy);

        /* Principal points */
        double goodCx,goodCy;
        fscanf(file,"%lf %lf",&goodCx,&goodCy);

        /* Read distortion */

        fscanf(file,"%lf",goodDistortion+0);
        fscanf(file,"%lf",goodDistortion+1);
        fscanf(file,"%lf",goodDistortion+2);
        fscanf(file,"%lf",goodDistortion+3);

        /* Read good Rot matrixes */
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            for( i = 0; i < 3; i++ )
                for( j = 0; j < 3; j++ )
                    fscanf(file, "%lf", goodRotMatrs + currImage * 9 + j * 3 + i);
        }

        /* Read good Trans vectors */
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            for( i = 0; i < 3; i++ )
                fscanf(file, "%lf", goodTransVects + currImage * 3 + i);
        }
        
        calibFlags = 
                     //CV_CALIB_FIX_PRINCIPAL_POINT +
                     //CV_CALIB_ZERO_TANGENT_DIST +
                     //CV_CALIB_FIX_ASPECT_RATIO +
                     //CV_CALIB_USE_INTRINSIC_GUESS + 
                     0;
        memset( cameraMatrix, 0, 9*sizeof(cameraMatrix[0]) );
        cameraMatrix[0] = cameraMatrix[4] = 807.;
        cameraMatrix[2] = (imageSize.width - 1)*0.5;
        cameraMatrix[5] = (imageSize.height - 1)*0.5;
        cameraMatrix[8] = 1.;

        /* Now we can calibrate camera */
        cvCalibrateCamera_64f(  numImages,
                                numbers,
                                imageSize,
                                imagePoints,
                                objectPoints,
                                distortion,
                                cameraMatrix,
                                transVects,
                                rotMatrs,
                                calibFlags );

        /* ---- Reproject points to the image ---- */
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            int numPoints = etalonSize.width * etalonSize.height;
            cvProjectPointsSimple(  numPoints,
                                    objectPoints + currImage * numPoints,
                                    rotMatrs + currImage * 9,
                                    transVects + currImage * 3,
                                    cameraMatrix,
                                    distortion,
                                    reprojectPoints + currImage * numPoints);
        }


        /* ----- Compute reprojection error ----- */
        i = 0;
        double dx,dy;
        double rx,ry;
        double meanDx,meanDy;
        double maxDx = 0.0;
        double maxDy = 0.0;

        meanDx = 0;
        meanDy = 0;
        for( currImage = 0; currImage < numImages; currImage++ )
        {
            for( currPoint = 0; currPoint < etalonSize.width * etalonSize.height; currPoint++ )
            {
                rx = reprojectPoints[i].x;
                ry = reprojectPoints[i].y;
                dx = rx - imagePoints[i].x;
                dy = ry - imagePoints[i].y;

                meanDx += dx;
                meanDy += dy;

                dx = fabs(dx);
                dy = fabs(dy);

                if( dx > maxDx )
                    maxDx = dx;
                
                if( dy > maxDy )
                    maxDy = dy;
                i++;
            }
        }

        meanDx /= numImages * etalonSize.width * etalonSize.height;
        meanDy /= numImages * etalonSize.width * etalonSize.height;

        /* ========= Compare parameters ========= */

        /* ----- Compare focal lengths ----- */
        code = compare(cameraMatrix+0,&goodFcx,1,0.01,"fx");
        if( code < 0 )
            goto _exit_;

        code = compare(cameraMatrix+4,&goodFcy,1,0.01,"fy");
        if( code < 0 )
            goto _exit_;

        /* ----- Compare principal points ----- */
        code = compare(cameraMatrix+2,&goodCx,1,0.01,"cx");
        if( code < 0 )
            goto _exit_;

        code = compare(cameraMatrix+5,&goodCy,1,0.01,"cy");
        if( code < 0 )
            goto _exit_;

        /* ----- Compare distortion ----- */
        code = compare(distortion,goodDistortion,4,0.01,"[k1,k2,p1,p2]");
        if( code < 0 )
            goto _exit_;

        /* ----- Compare rot matrixs ----- */
        code = compare(rotMatrs,goodRotMatrs, 9*numImages,0.05,"rotation matrices");
        if( code < 0 )
            goto _exit_;

        /* ----- Compare rot matrixs ----- */
        code = compare(transVects,goodTransVects, 3*numImages,0.05,"translation vectors");
        if( code < 0 )
            goto _exit_;

        if( maxDx > 1.0 )
        {
            ts->printf( CvTS::LOG,
                      "Error in reprojection maxDx=%f > 1.0\n",maxDx);
            code = CvTS::FAIL_BAD_ACCURACY; goto _exit_;
        }

        if( maxDy > 1.0 )
        {
            ts->printf( CvTS::LOG,
                      "Error in reprojection maxDy=%f > 1.0\n",maxDy);
            code = CvTS::FAIL_BAD_ACCURACY; goto _exit_;
        }

        cvFree(&imagePoints);
        cvFree(&objectPoints);
        cvFree(&reprojectPoints);
        cvFree(&numbers);

        cvFree(&transVects);
        cvFree(&rotMatrs);
        cvFree(&goodTransVects);
        cvFree(&goodRotMatrs);

        fclose(file);
        file = 0;
    }

_exit_:

    if( file )
        fclose(file);

    if( datafile )
        fclose(datafile);

    /* Free all allocated memory */
    cvFree(&imagePoints);
    cvFree(&objectPoints);
    cvFree(&reprojectPoints);
    cvFree(&numbers);

    cvFree(&transVects);
    cvFree(&rotMatrs);
    cvFree(&goodTransVects);
    cvFree(&goodRotMatrs);

    if( code < 0 )
        ts->set_failed_test_info( code );
}

//CV_BundleAdjustmentTest bundleadjustment_test;

#endif