/**

	Copyright (c) 2011, C2Serve (http://www.c2serve.eu)
	All rights reserved.

	Redistribution and use in source and binary forms, with or without
	modification, are permitted provided that the following conditions are met:
	1. Redistributions of source code must retain the above copyright
	   notice, this list of conditions and the following disclaimer.
	2. Redistributions 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.
	3. All advertising materials mentioning features or use of this software
	   must display the following acknowledgement: "This product includes software of the C2Serve project".
	4. Neither the name of the C2Serve project nor the
	   names of its contributors may be used to endorse or promote products
	   derived from this software without specific prior written permission.

	THIS SOFTWARE IS PROVIDED BY C2SERVE ''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 C2SERVE 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.

 */

#include "C2STestRestMethodAdd.h"

#include "C2STestRestEntityStreamerXML.h"
#include "C2STestRestEntityStreamerJSON.h"

#include <boost/test/unit_test.hpp>

namespace c2s
{
  namespace test
  {

    const std::string C2STestRestMethodAdd::sPath = "math/add";

    C2STestRestMethodAdd::C2STestRestMethodAdd()
      : C2SRestMethodPrototypeGET<unsigned int>( sPath ),
        m_iArg1( 0 ),
        m_iArg2( 0 ),
        m_iMultiplier( 0 )
    {
      C2SRestMethodPrototypeGET<unsigned int>::addPathParameter( "arg1" , &m_iArg1 );
      C2SRestMethodPrototypeGET<unsigned int>::addPathParameter( "arg2" , &m_iArg2 );
      C2SRestMethodPrototypeGET<unsigned int>::addQueryParameter( "multiplier" , &m_iMultiplier , m_iMultiplier );
      C2SRestMethodPrototypeGET<unsigned int>::installEntityStreamer( new C2STestRestEntityStreamerXML<unsigned int>() );
      C2SRestMethodPrototypeGET<unsigned int>::installEntityStreamer( new C2STestRestEntityStreamerJSON<unsigned int>() );
    };

    C2SHttpResponse *C2STestRestMethodAdd::process()
    {
      BOOST_MESSAGE( "C2STestRestMethodAdder::process: " << m_iArg1 << " + " << m_iArg2 << " * " << m_iMultiplier );
      unsigned int iResult = ( m_iArg1 + m_iArg2 ) * m_iMultiplier;
      return C2SRestMethodPrototypeGET<unsigned int>::buildResponse( OK , iResult );
    }

    C2STestRestMethodAdd *C2STestRestMethodAdd::clone() const
    {
      return new C2STestRestMethodAdd();
    }

  }
}


//
// Created by jachu on 27.06.17.
//

#ifndef PLANELOC_LINESEG_HPP
#define PLANELOC_LINESEG_HPP

#include <vector>

//#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>

#include "Types.hpp"
#include "Serialization.hpp"

class LineSeg {
public:
    LineSeg();

    LineSeg(int iframeId,
            const Eigen::Vector2d &ipi1,
            const Eigen::Vector2d &ipi2,
            const Eigen::Vector3d &ip1,
            const Eigen::Vector3d &ip2);

    const std::vector<int> &getFrameIds() const;

    const vectorVector2d &getPis1() const;

    const vectorVector2d &getPis2() const;

    const Eigen::Vector3d &getP1() const;

    const Eigen::Vector3d &getP2() const;

    void setP1(const Eigen::Vector3d &p1);

    void setP2(const Eigen::Vector3d &p2);

    Vector6d toPointNormalEq() const;

    Vector7d toSE3Point() const;

    LineSeg transformed(const Vector7d &transform) const;

    double eqDist(const LineSeg &ls);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    std::vector<int> frameIds;

    vectorVector2d pis1, pis2;

    Eigen::Vector3d p1, p2;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & frameIds;
        ar & pis1;
        ar & pis2;
        ar & p1;
        ar & p2;
    }
};

#endif //PLANELOC_LINESEG_HPP


/*
 * rsxgltest - framebuffer object demo. A cube within a cube, maaaaaaan!
 */

#define GL3_PROTOTYPES
#include <GL3/gl3.h>
#include <GL3/gl3ext.h>

#include "rsxgltest.h"
#include "math3d.h"
#include "sine_wave.h"

#include <stddef.h>
#include "fbo_inner_vert.h"
#include "fbo_inner_frag.h"

#include <io/pad.h>

#include <math.h>
#include <Eigen/Geometry>

#include "texture.h"
#include "nagel_bin.h"

const char * rsxgltest_name = "copyteximage";

struct sine_wave_t rgb_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f
  },
  { 0.5f,
    0.5f,
    1.5f
  },
  { 0.5f,
    0.5f,
    2.5f
  }
};

struct sine_wave_t xyz_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f / 4.0f
  },
  { 0.5f,
    0.5f,
    1.5f / 4.0f
  },
  { 0.5f,
    0.5f,
    2.5f / 4.0f
  }
};

GLuint vao = 0;
GLuint buffers[2] = { 0,0 };

Image image;
GLuint textures[2] = { 0,0 };
GLuint program = 0;

GLint ProjMatrix_location = -1, TransMatrix_location = -1, NormalMatrix_location = -1,
  vertex_location = 0, normal_location = 1, tc_location = 2,
  light_location = -1;

const float geometry[] = {
    // -X
    -0.5,-0.5,-0.5,
    -1.0,0.0,0.0,
    0,0,

    -0.5,0.5,-0.5,
    -1.0,0.0,0.0,
    1,0,

    -0.5,0.5,0.5,
    -1.0,0.0,0.0,
    1,1,

    -0.5,-0.5,0.5,
    -1.0,0.0,0.0,
    0,1,

    // +X
    0.5,-0.5,-0.5,
    1.0,0.0,0.0,
    0,0,

    0.5,-0.5,0.5,
    1.0,0.0,0.0,
    1,0,

    0.5,0.5,0.5,
    1.0,0.0,0.0,
    1,1,

    0.5,0.5,-0.5,
    1.0,0.0,0.0,
    0,1,

    // -Y
    -0.5,-0.5,-0.5,
    0.0,-1.0,0.0,
    0,0,

    -0.5,-0.5,0.5,
    0.0,-1.0,0.0,
    1,0,

    0.5,-0.5,0.5,
    0.0,-1.0,0.0,
    1,1,

    0.5,-0.5,-0.5,
    0.0,-1.0,0.0,
    0,1,

    // +Y
    -0.5,0.5,-0.5,
    0.0,1.0,0.0,
    0,0,

    0.5,0.5,-0.5,
    0.0,1.0,0.0,
    1,0,

    0.5,0.5,0.5,
    0.0,1.0,0.0,
    1,1,

    -0.5,0.5,0.5,
    0.0,1.0,0.0,
    0,1,

    // -Z
    -0.5,-0.5,-0.5,
    0.0,0.0,-1.0,
    0,0,

    -0.5,0.5,-0.5,
    0.0,0.0,-1.0,
    1,0,

    0.5,0.5,-0.5,
    0.0,0.0,-1.0,
    1,1,

    0.5,-0.5,-0.5,
    0.0,0.0,-1.0,
    0,1,

    // +Z
    -0.5,-0.5,0.5,
    0.0,0.0,1.0,
    0,0,

    -0.5,0.5,0.5,
    0.0,0.0,1.0,
    1,0,

    0.5,0.5,0.5,
    0.0,0.0,1.0,
    1,1,

    0.5,-0.5,0.5,
    0.0,0.0,1.0,
    0,1
  };

  const GLuint indices[] = {
    // -X
    0, 1, 2,
    2, 3, 0,

    // +X
    4, 5, 6,
    6, 7, 4,

    // -Y
    8, 9, 10,
    10, 11, 8,

    // +Y
    12, 13, 14,
    14, 15, 12,

    // -Z
    16, 17, 18,
    18, 19, 16,

    // +Z
    20, 21, 22,
    22, 23, 20
  };

GLuint * client_indices = 0;

#define DTOR(X) ((X)*0.01745329f)
#define RTOD(d) ((d)*57.295788f)

// Projection matrix for the outer cube:
Eigen::Projective3f ProjMatrix(perspective(DTOR(54.3),1920.0 / 1080.0,0.1,1000.0));

// Projection matrix for the inner cube - the image that gets rendered to the FBO:
Eigen::Projective3f ProjMatrix2(perspective(DTOR(35.3),1920.0 / 1080.0,0.1,1000.0));

Eigen::Affine3f ViewMatrixInv =
  Eigen::Affine3f(Eigen::Affine3f::Identity() *
		  (
		   Eigen::Translation3f(1.779,2.221,4.034) *
		   (
		    Eigen::AngleAxisf(DTOR(0),Eigen::Vector3f::UnitZ()) *
		    Eigen::AngleAxisf(DTOR(23.8),Eigen::Vector3f::UnitY()) *
		    Eigen::AngleAxisf(DTOR(-26.738),Eigen::Vector3f::UnitX())
		    )
		   )
		  ).inverse();

const GLfloat light[4] = { 10.0f, 10.0f, 10.0f, 1.0f };

extern "C"
void
rsxgltest_pad(unsigned int,const padData * paddata)
{
  if(paddata -> BTN_UP) {
    tcp_printf("up\n");
  }
  else if(paddata -> BTN_DOWN) {
    tcp_printf("down\n");
  }
}

extern "C"
void
rsxgltest_init(int argc,const char ** argv)
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);

  // Set up us the program:
  GLuint shaders[2] = { 0,0 };

  {
    shaders[0] = glCreateShader(GL_VERTEX_SHADER);
    shaders[1] = glCreateShader(GL_FRAGMENT_SHADER);

    program = glCreateProgram();

    glAttachShader(program,shaders[0]);
    glAttachShader(program,shaders[1]);

#if 0
    // Supply shader binaries:
    glShaderBinary(1,shaders,0,fbo_inner_vpo,fbo_inner_vpo_size);
    glShaderBinary(1,shaders + 1,0,fbo_inner_fpo,fbo_inner_fpo_size);
#endif

    // Supply shader SOURCES!
    const GLchar * shader_srcs[] = { (const GLchar *)fbo_inner_vert, (const GLchar *)fbo_inner_frag };
    GLint shader_srcs_lengths[] = { fbo_inner_vert_len, fbo_inner_frag_len };
    GLint compiled = 0;

    glShaderSource(shaders[0],1,shader_srcs,shader_srcs_lengths);
    glCompileShader(shaders[0]);

    glGetShaderiv(shaders[0],GL_COMPILE_STATUS,&compiled);
    tcp_printf("shader compile status: %i\n",compiled);

    glShaderSource(shaders[1],1,shader_srcs + 1,shader_srcs_lengths + 1);
    glCompileShader(shaders[1]);

    glGetShaderiv(shaders[1],GL_COMPILE_STATUS,&compiled);
    tcp_printf("shader compile status: %i\n",compiled);

    //glBindAttribLocation(program,vertex_location,"vertex");
    //glBindAttribLocation(program,normal_location,"normal");
    //glBindAttribLocation(program,tc_location,"uv");

    // Link the program for real:
    glLinkProgram(program);
    glValidateProgram(program);

    summarize_program("draw",program);

    vertex_location = glGetAttribLocation(program,"vertex");
    normal_location = glGetAttribLocation(program,"normal");
    tc_location = glGetAttribLocation(program,"uv");

    tcp_printf("attrib locations: %i %i %i\n",
	       glGetAttribLocation(program,"vertex"),
	       glGetAttribLocation(program,"normal"),
	       glGetAttribLocation(program,"uv"));

    ProjMatrix_location = glGetUniformLocation(program,"ProjMatrix");
    TransMatrix_location = glGetUniformLocation(program,"TransMatrix");
    NormalMatrix_location = glGetUniformLocation(program,"NormalMatrix");
    GLint texture_location = glGetUniformLocation(program,"texture");
    light_location = glGetUniformLocation(program,"light");

    tcp_printf("vertex_location: %i\n",vertex_location);
    tcp_printf("normal_location: %i\n",normal_location);
    tcp_printf("tc_location: %i\n",tc_location);
    tcp_printf("ProjMatrix_location: %i TransMatrix_location: %i\n",
	       ProjMatrix_location,TransMatrix_location);
    tcp_printf("texture_location: %i\n",texture_location);

    glUseProgram(program);

    glUniform1i(texture_location,0);
    glUniform4fv(light_location,1,light);
  }

  // Set up us the vertex data:
  glGenBuffers(2,buffers);

  glBindBuffer(GL_ARRAY_BUFFER,buffers[0]);

  glBufferData(GL_ARRAY_BUFFER,sizeof(float) * 6 * 4 * 8,geometry,GL_STATIC_DRAW);
  glEnableVertexAttribArray(vertex_location);
  glEnableVertexAttribArray(normal_location);
  glEnableVertexAttribArray(tc_location);
  glVertexAttribPointer(vertex_location,3,GL_FLOAT,GL_FALSE,sizeof(float) * 8,0);
  glVertexAttribPointer(normal_location,3,GL_FLOAT,GL_FALSE,sizeof(float) * 8,(const GLvoid *)(sizeof(float) * 3));
  glVertexAttribPointer(tc_location,2,GL_FLOAT,GL_FALSE,sizeof(float) * 8,(const GLvoid *)(sizeof(float) * 6));

  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,buffers[1]);
  glBufferData(GL_ELEMENT_ARRAY_BUFFER,sizeof(GLuint) * 6 * 2 * 3,indices,GL_STATIC_DRAW);

  client_indices = (GLuint *)malloc(sizeof(GLuint) * 6 * 2 * 3);
  memcpy(client_indices,indices,sizeof(GLuint) * 6 * 2 * 3);

  // Texture map:
  image = loadPng(nagel_bin);
  tcp_printf("image size: %u %u\n",image.width,image.height);

  glGenTextures(2,textures);

  // image asset:
  glBindTexture(GL_TEXTURE_2D,textures[0]);

  glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,image.width,image.height,0,GL_BGRA,GL_UNSIGNED_BYTE,image.data);

  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);

  // rendering surface:
  glBindTexture(GL_TEXTURE_2D,textures[1]);

  glTexStorage2D(GL_TEXTURE_2D,1,GL_RGBA,image.width,image.height);
  //glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,image.width,image.height,0,GL_BGRA,GL_UNSIGNED_BYTE,0);

  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);

  glBindTexture(GL_TEXTURE_2D,0);
}

extern "C"
int
rsxgltest_draw()
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

#if 0
  float rgb[3] = {
    compute_sine_wave(rgb_waves,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 2,rsxgltest_elapsed_time)
  };
#endif

  float tmp = 0.0;
  const float elapsed_time_bin = modff(rsxgltest_elapsed_time / 9.0f,&tmp);
  float rgb[3] = {
    (elapsed_time_bin < (1.0f / 3.0f)) ? 1.0f : 0.0f,
     (elapsed_time_bin >= (1.0f / 3.0f) && elapsed_time_bin < (2.0f / 3.0f)) ? 1.0f : 0.0f,
     (elapsed_time_bin >= (2.0f / 3.0f) && elapsed_time_bin < 1.0f) ? 1.0f : 0.0f
     };

  float xyz[3] = {
    compute_sine_wave(xyz_waves,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 2,rsxgltest_elapsed_time)
  };

  Eigen::Affine3f rotmat =
    Eigen::Affine3f::Identity() *
    Eigen::AngleAxisf(DTOR(xyz[2]) * 360.0f,Eigen::Vector3f::UnitZ()) *
    Eigen::AngleAxisf(DTOR(xyz[1]) * 360.0f,Eigen::Vector3f::UnitY()) *
    Eigen::AngleAxisf(DTOR(xyz[0]) * 360.0f,Eigen::Vector3f::UnitX());

  //Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * rotmat * Eigen::UniformScaling< float >(3.0));

  Transform3f modelview = ViewMatrixInv * (Transform3f::Identity() * rotmat * Eigen::UniformScaling< float >(3.0));

  glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());

  Eigen::Affine3f normal = Eigen::Affine3f::Identity();
  normal.linear() = modelview.linear().inverse().transpose();
  glUniformMatrix4fv(NormalMatrix_location,1,GL_FALSE,normal.data());

  //
  glViewport(0,0,image.width,image.height);

  glClearColor(1.0 - rgb[0],1.0 - rgb[1],1.0 - rgb[2],1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glBindTexture(GL_TEXTURE_2D,textures[0]);

  {
    glUniformMatrix4fv(ProjMatrix_location,1,GL_FALSE,ProjMatrix2.data());
    glDrawElements(GL_TRIANGLES,36,GL_UNSIGNED_INT,0 /*client_indices*/);
  }

  glBindTexture(GL_TEXTURE_2D,textures[1]);
  //glCopyTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,0,0,image.width,image.height,0);
  glCopyTexSubImage2D(GL_TEXTURE_2D,0,0,0,0,0,image.width,image.height);

  //
  glViewport(0,0,rsxgltest_width,rsxgltest_height);

  glClearColor(rgb[0],rgb[1],rgb[2],1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glBindTexture(GL_TEXTURE_2D,textures[1]);

  {
    glUniformMatrix4fv(ProjMatrix_location,1,GL_FALSE,ProjMatrix.data());
    glDrawElements(GL_TRIANGLES,36,GL_UNSIGNED_INT,0 /*client_indices*/);
  }

  return 1;
}

extern "C"
void
rsxgltest_exit()
{
  glBindBuffer(GL_ARRAY_BUFFER,0);

  glVertexAttribPointer(vertex_location,3,GL_FLOAT,GL_FALSE,0,0);
  glVertexAttribPointer(normal_location,3,GL_FLOAT,GL_FALSE,0,0);
  glVertexAttribPointer(tc_location,2,GL_FLOAT,GL_FALSE,0,0);

  glDeleteBuffers(2,buffers);

  glDeleteProgram(program);
}


#include <iostream>
#include <math.h>
#include "properties.hpp"
#include "constants.hpp"
#include "bomd_rescale.hpp"
#include "qm_qchem.hpp"
//
#include <armadillo>
//

void
RescaleBomd::get_reader_data(ConfigBlockReader& reader) {
//    reader.get_data("additional_energy", additional_energy);
//    reader.get_data("step", step);
//    reader.get_data("dE", dE);
    (void) reader;
};

ConfigBlockReader
RescaleBomd::setup_reader() {
    //using types = ConfigBlockReader::types;
    std::cout << "Setup Rescale BOMD\n";
    ConfigBlockReader reader{"bomd-rescale"};
    //
    //reader.add_entry("active_state", 0);
    /*
    reader.add_entry("additional_energy", 1.0);
    reader.add_entry("dE", 0.01);
    reader.add_entry("step", 1);
    */
    return reader;
};

double
RescaleBomd::update_gradient()
{
    qm->update();

    /*
    PropMap props{};
    props.emplace(QMProperty::qmgradient, &qm_grd);
    props.emplace(QMProperty::mmgradient, &mm_grd);
    props.emplace(QMProperty::energies, &energy);
    //

    qm->get_properties(props);
    */
    //
    qm_grd.fill(0.0);
    mm_grd.fill(0.0);
    //
    double ene = 0.0; //energy[0];
    if (additional_energy > 0.0) {
       ene += additional_energy;
    }
    total_energy = ene;
    return ene;
};

double
kinetic_energy(arma::mat &velocities, arma::vec &masses) {
    double ekin = 0.0;
    for (arma::uword i=0; i<masses.n_elem; ++i) {
        for (arma::uword j=0; j<3; ++j) {
            ekin += 0.5 * velocities(j, i) * velocities(j, i)  * masses[i];
        }
    }
    return ekin;
}

bool RescaleBomd::rescale_velocities(arma::mat &velocities, arma::vec &masses, arma::mat &total_gradient, double e_drift) {
  static int ncall = 1;
  bool retval = false;

  (void) total_gradient;
  (void) e_drift;

  const double ekin = kinetic_energy(velocities, masses);
  const double factor = 1.0 + dE/ekin;
  //
  if ((ncall % step) == 0) {
    std::cout << "Rescale kinetic energy \n";
    std::cout << "factor: " << factor << "\n";
    if (additional_energy > dE) {
        std::cout << "we actually scale!\n";
        velocities *= sqrt(factor);
        additional_energy -= dE;
    }
    retval = true;
  }
  ncall += 1;
  std::cout << "Total energy " << total_energy << "au " << " ekin = " << ekin << "\n";
  return retval;
};


// Copyright 2019 Josh Pieper, jjp@pobox.com.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "base/sophus.h"

#include <boost/test/auto_unit_test.hpp>

#include "mjlib/base/json5_write_archive.h"
#include "mjlib/base/json5_read_archive.h"

BOOST_AUTO_TEST_CASE(BasicSophusTest) {
  Sophus::SE3d zero;
  const auto actual = mjlib::base::Json5WriteArchive::Write(zero);
  const std::string expected = R"XXX({
    "so3" : {
      "x" : 0,
      "y" : 0,
      "z" : 0,
      "w" : 1
    },
    "translation" : [
      0,
      0,
      0
    ]
  })XXX";
  BOOST_TEST(actual == expected);
}


#ifndef __fovis_stereo_rectify_hpp__
#define __fovis_stereo_rectify_hpp__

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "camera_intrinsics.hpp"

namespace fovis
{

void
stereo_rectify(const CameraIntrinsicsParameters& left_params,
               const CameraIntrinsicsParameters& right_params,
               const Eigen::Quaterniond& rotation_quat,
               const Eigen::Vector3d& translation,
               Eigen::Matrix3d* left_rotation,
               Eigen::Matrix3d* right_rotation,
               CameraIntrinsicsParameters* rectified_params);
}

#endif