//
// file NotHashedFingerprint.cc
// David Cosgrove
// AstraZeneca
// 3rd February 2009
//

#include <fstream>
#include <iostream>
#include <set>
#include <sstream>

#include <boost/lexical_cast.hpp>

#include "ByteSwapper.H"
#include "NotHashedFingerprint.H"

using namespace std;

namespace DAC_FINGERPRINTS {

  pNHDC NotHashedFingerprint::dist_calc_ = &NotHashedFingerprint::tanimoto;
  pNHTDC NotHashedFingerprint::threshold_dist_calc_ =
    &NotHashedFingerprint::tanimoto;

  // ****************************************************************************
  NotHashedFingerprint::NotHashedFingerprint() :
  FingerprintBase() , num_frag_nums_( 0 ) , frag_nums_( 0 ) {

  }

  // ****************************************************************************
  NotHashedFingerprint::NotHashedFingerprint( const string &name ) :
    FingerprintBase( name ) , num_frag_nums_( 0 ) , frag_nums_( 0 ) {

  }

  // ****************************************************************************
  // from the product of get_string_rep.
  NotHashedFingerprint::NotHashedFingerprint( const string &name ,
					      const string &rep ) :
    FingerprintBase( name ) , num_frag_nums_( 0 ) , frag_nums_( 0 ) {

    vector<uint32_t> tmp_bits;
    istringstream iss( rep.substr( 4 ) );
    while( 1 ) {
      uint32_t next_bit;
      iss >> next_bit;
      if( iss.fail() ) {
	break;
      }
      tmp_bits.push_back( next_bit );
    }

    build_from_vector( tmp_bits );

  }

  // ****************************************************************************
  NotHashedFingerprint::NotHashedFingerprint( const string &name ,
					      const std::vector<uint32_t> in_nums ) :
    FingerprintBase( name ) , num_frag_nums_( 0 ) , frag_nums_( 0 ) {

    build_from_vector( in_nums );

  }

  // ****************************************************************************
  NotHashedFingerprint::NotHashedFingerprint( const NotHashedFingerprint &fp ) :
    FingerprintBase() , num_frag_nums_( 0 ) , frag_nums_( 0 ) {

    copy_data( fp );

  }

  // ****************************************************************************
  NotHashedFingerprint::~NotHashedFingerprint() {

    delete [] frag_nums_;

  }

  // ****************************************************************************
  NotHashedFingerprint &NotHashedFingerprint::operator=( const NotHashedFingerprint &fp ) {

    if( this == &fp ) {
      return *this;
    }

    copy_data( fp );

    return *this;

  }

  // *******************************************************************************
  // bitwise operators for combining fingerprints.
  NotHashedFingerprint NotHashedFingerprint::operator&( const NotHashedFingerprint &rhs ) const {

    NotHashedFingerprint ret_val( "" );

    vector<uint32_t> v1( frag_nums_ , frag_nums_ + num_frag_nums_ );
    sort( v1.begin() , v1.end() );

    vector<uint32_t> v2( rhs.frag_nums_ , rhs.frag_nums_ + rhs.num_frag_nums_ );
    sort( v2.begin() , v2.end() );

    vector<uint32_t> v1_or_2( num_frag_nums_ + rhs.num_frag_nums_ , 0 );
    vector<uint32_t>::iterator un_end =
      set_intersection( v1.begin() , v1.end() , v2.begin() ,
			v2.end() , v1_or_2.begin() );

    delete [] ret_val.frag_nums_;
    ret_val.num_frag_nums_ = distance( v1_or_2.begin() , un_end );
    ret_val.frag_nums_ = new uint32_t[ret_val.num_frag_nums_];
    copy( v1_or_2.begin() , un_end , ret_val.frag_nums_ );

    return ret_val;

  }

  // *******************************************************************************
  NotHashedFingerprint NotHashedFingerprint::operator|( const NotHashedFingerprint &rhs ) const {

    NotHashedFingerprint ret_val( "" );

    vector<uint32_t> v1( frag_nums_ , frag_nums_ + num_frag_nums_ );
    sort( v1.begin() , v1.end() );

    vector<uint32_t> v2( rhs.frag_nums_ , rhs.frag_nums_ + rhs.num_frag_nums_ );
    sort( v2.begin() , v2.end() );

    vector<uint32_t> v1_or_2( num_frag_nums_ + rhs.num_frag_nums_ , 0 );
    vector<uint32_t>::iterator un_end =
      set_union( v1.begin() , v1.end() , v2.begin() , v2.end() , v1_or_2.begin() );

    delete [] ret_val.frag_nums_;
    ret_val.num_frag_nums_ = distance( v1_or_2.begin() , un_end );
    ret_val.frag_nums_ = new uint32_t[ret_val.num_frag_nums_];
    copy( v1_or_2.begin() , un_end , ret_val.frag_nums_ );

    return ret_val;

  }

  // ****************************************************************************
  NotHashedFingerprint &NotHashedFingerprint::operator&=( const NotHashedFingerprint &rhs ) {

    vector<uint32_t> v1( frag_nums_ , frag_nums_ + num_frag_nums_ );
    sort( v1.begin() , v1.end() );

    vector<uint32_t> v2( rhs.frag_nums_ , rhs.frag_nums_ + rhs.num_frag_nums_ );
    sort( v2.begin() , v2.end() );

    vector<uint32_t> v1_or_2( num_frag_nums_ + rhs.num_frag_nums_ , 0 );
    vector<uint32_t>::iterator un_end =
      set_intersection( v1.begin() , v1.end() , v2.begin() , v2.end() ,
			v1_or_2.begin() );

    delete [] frag_nums_;
    num_frag_nums_ = distance( v1_or_2.begin() , un_end );
    frag_nums_ = new uint32_t[num_frag_nums_];
    copy( v1_or_2.begin() , un_end , frag_nums_ );

    return *this;

  }

  // ****************************************************************************
  NotHashedFingerprint &NotHashedFingerprint::operator|=( const NotHashedFingerprint &rhs ) {

    vector<uint32_t> v1( frag_nums_ , frag_nums_ + num_frag_nums_ );
    sort( v1.begin() , v1.end() );

    vector<uint32_t> v2( rhs.frag_nums_ , rhs.frag_nums_ + rhs.num_frag_nums_ );
    sort( v2.begin() , v2.end() );

    vector<uint32_t> v1_or_2( num_frag_nums_ + rhs.num_frag_nums_ , 0 );
    vector<uint32_t>::iterator un_end =
      set_union( v1.begin() , v1.end() , v2.begin() , v2.end() , v1_or_2.begin() );

    delete [] frag_nums_;
    num_frag_nums_ = distance( v1_or_2.begin() , un_end );
    frag_nums_ = new uint32_t[num_frag_nums_];
    copy( v1_or_2.begin() , un_end , frag_nums_ );

    return *this;

  }

  // ****************************************************************************
  double NotHashedFingerprint::tanimoto( const NotHashedFingerprint &fp ) const {

    int num_comm = num_bits_in_common( fp );
    return( 1.0 - ( double( num_comm ) /
		    double( num_frag_nums_ + fp.num_frag_nums_ - num_comm ) ) );

  }

  // ****************************************************************************
  double NotHashedFingerprint::tanimoto( const NotHashedFingerprint &f ,
					 float thresh ) const {

    float min_dist;
    if( num_frag_nums_ < f.num_frag_nums_ )
      min_dist = 1.0 - float( num_frag_nums_ ) / float( f.num_frag_nums_ );
    else
      min_dist = 1.0 - float( f.num_frag_nums_ ) / float( num_frag_nums_ );
    if( min_dist > thresh ) {
      return 1.0;
    } else
      return tanimoto( f );

  }

  // ****************************************************************************
  double NotHashedFingerprint::tversky( const NotHashedFingerprint &f ) const {

    int num_in_a_not_b , num_in_b_not_a;
    int num_in_common = num_bits_in_common( f , num_in_a_not_b , num_in_b_not_a );

    double dist = 1.0 - ( double( num_in_common ) /
			  ( tversky_alpha_ * double( num_in_a_not_b ) +
			    ( 1.0 - tversky_alpha_ ) * double( num_in_b_not_a )
			    + double( num_in_common ) ) );

    return dist;

  }

  // ****************************************************************************
  double NotHashedFingerprint::tversky( const NotHashedFingerprint &f ,
                                        float thresh __attribute__((unused)) ) const {

    return tversky( f );

  }

  // ****************************************************************************
  void NotHashedFingerprint::binary_write( gzFile fp ) const {

    int name_len = finger_name_.length();
    gzwrite( fp , reinterpret_cast<char *>( &name_len ) , sizeof( int ) );
    gzwrite( fp , &finger_name_[0] , name_len );

    gzwrite( fp , reinterpret_cast<const char *>( &num_frag_nums_ ) ,
	     sizeof( int ) );
    gzwrite( fp , reinterpret_cast<const char *>( frag_nums_ ) ,
	     num_frag_nums_ * sizeof( uint32_t ) );

  }

  // ****************************************************************************
  void NotHashedFingerprint::binary_write( FILE *fp ) const {

    int name_len = finger_name_.length();
    fwrite( reinterpret_cast<char *>( &name_len ) , sizeof( int ) , 1 , fp );
    fwrite( &finger_name_[0] , 1 , name_len , fp );

    fwrite( reinterpret_cast<const char *>( &num_frag_nums_ ) , sizeof( int ) ,
	    1 , fp );
    fwrite( reinterpret_cast<const char *>( frag_nums_ ) , sizeof( uint32_t ) ,
	    num_frag_nums_ , fp );

  }

  // ****************************************************************************
  bool NotHashedFingerprint::binary_read( gzFile fp , bool byte_swapping ) {

    int name_len;
    gzread( fp , reinterpret_cast<void *>( &name_len ) , sizeof( int ) );
    if( gzeof( fp ) ) {
      return false;
    }
    if( byte_swapping ) {
      DACLIB::byte_swapper<int>( name_len );
    }
    finger_name_.resize( name_len , ' ' );
    gzread( fp , reinterpret_cast<void *>( &finger_name_[0] ) , name_len );

    gzread( fp , reinterpret_cast<void *>( &num_frag_nums_ ) , sizeof( int ) );
    if( byte_swapping ) {
      DACLIB::byte_swapper<int>( num_frag_nums_ );
    }

    delete [] frag_nums_;
    if( num_frag_nums_ ) {
      frag_nums_ = new uint32_t[num_frag_nums_];
      gzread( fp , reinterpret_cast<void *>( frag_nums_ ) ,
	      num_frag_nums_ * sizeof( uint32_t ) );
    } else {
      frag_nums_ = 0;
    }

    return true;

  }

  // ****************************************************************************
  bool NotHashedFingerprint::ascii_read( gzFile fp , const string &sep ) {

    // in FingerprintBase.cc
    string full_line = read_full_line( fp );
    if( gzeof( fp ) && full_line.empty() ) {
      return false;
    }

    full_line = convert_sep_to_new_sep( full_line , sep , " " );
    istringstream iss( full_line );
    iss >> finger_name_;

    vector<uint32_t> fns;
    while( 1 ) {
      uint32_t next_fn;
      iss >> next_fn;
      if( iss.fail() ) {
	break;
      }
      fns.push_back( next_fn );
    }

    build_from_vector( fns );

    return true;

  }

  // ****************************************************************************
  void NotHashedFingerprint::ascii_write( gzFile fp , const string &sep ) const {

    string act_sep = sep.empty() ? " " : sep;
    gzprintf( fp , "%s" , finger_name_.c_str() );

    // cowardly skirting round of the issue of what format string to use for
    // uint32_t in printf!  There seems to be a buffer size issue here,
    // strings greater than 4K are being truncated, so write each frag_num_
    // separately.
    ostringstream oss;
    for( int i = 0 ; i < num_frag_nums_ ; ++i ) {
      oss << act_sep << frag_nums_[i];
      gzprintf( fp , "%s" , oss.str().c_str() );
      oss.str( "" );
    }
    gzprintf( fp , "\n" );

  }

  // ****************************************************************************
  void NotHashedFingerprint::ascii_write( FILE *fp , const string &sep ) const {

    string act_sep = sep.empty() ? " " : sep;
    fprintf( fp , "%s" , finger_name_.c_str() );

    // cowardly skirting round of the issue of what format chars to use for
    // uint32_t in printf!
    ostringstream oss;
    for( int i = 0 ; i < num_frag_nums_ ; ++i ) {
      oss << act_sep << frag_nums_[i];
    }
    fprintf( fp , "%s" , oss.str().c_str() );
    fprintf( fp , "\n" );

  }

  // ****************************************************************************
  string NotHashedFingerprint::get_string_rep() const {

    ostringstream os;
    os << "__FN";
    for( int i = 0 ; i < num_frag_nums_ - 1 ; ++i ) {
      os << frag_nums_[i] << " ";
    }
    if( num_frag_nums_ ) {
      os << frag_nums_[num_frag_nums_ - 1];
    }

    return os.str();

  }

  // ****************************************************************************
  // count the number of bits in common between the fingerprint passed in
  // and this one
  int NotHashedFingerprint::num_bits_in_common( const NotHashedFingerprint &fp ) const {

    // both sets of frag_nums are sorted, so can walk through them in sequence
    uint32_t *these = frag_nums_ , *those = fp.frag_nums_;
    uint32_t *these_stop = frag_nums_ + num_frag_nums_;
    uint32_t *those_stop = fp.frag_nums_ + fp.num_frag_nums_;
    int num_comm = 0;
    while( these != these_stop && those != those_stop ) {
      if( *these < *those ) {
	while( these != these_stop && *these < *those ) {
	  ++these;
	}
      } else {
	while( those != those_stop && *those < *these ) {
	  ++those;
	}
      }
      if( these == these_stop || those == those_stop ) {
	break;
      }
      if( *these == *those ) {
	++num_comm;
	++these;
	++those;
      }
    }

    return num_comm;

  }

  // ****************************************************************************
  // count the number of bits in common between the fingerprint passed in
  // and this one
  int NotHashedFingerprint::num_bits_in_common( const NotHashedFingerprint &fp ,
						int &num_in_a_not_b ,
						int &num_in_b_not_a ) const {

    num_in_a_not_b = num_in_b_not_a = 0;

    // both sets of frag_nums are sorted, so can walk through them in sequence
    uint32_t *these = frag_nums_ , *those = fp.frag_nums_;
    uint32_t *these_stop = frag_nums_ + num_frag_nums_;
    uint32_t *those_stop = fp.frag_nums_ + fp.num_frag_nums_;
    int num_comm = 0;
    while( these != these_stop && those != those_stop ) {
      // move these or those ( whichever has lower value) until they're equal
      // or one hits the end
      if( *these < *those ) {
	while( *these < *those ) {
	  ++these;
	  ++those;
	  ++num_in_a_not_b;
	}
      } else {
	while( *those < *these ) {
	  ++these;
	  ++those;
	  ++num_in_b_not_a;
	}
      }
      if( those == those_stop ) {
	while( these < these_stop ) {
	  ++these;
	  ++num_in_a_not_b;
	}
	break;
      }
      if( these == these_stop ) {
	while( those < those_stop ) {
	  ++these;
	  ++num_in_b_not_a;
	}
	break;
      }
      if( *those == *these ) {
	++num_comm;
      }
      ++these;
      ++those;
    }

    return num_comm;

  }

  // *************************************************************************
  void NotHashedFingerprint::set_similarity_calc( SIMILARITY_CALC sc ) {

    switch( sc ) {
      case TANIMOTO :
	dist_calc_ = &NotHashedFingerprint::tanimoto;
	threshold_dist_calc_ = &NotHashedFingerprint::tanimoto;
	break;
      case TVERSKY :
	dist_calc_ = &NotHashedFingerprint::tversky;
	threshold_dist_calc_ = &NotHashedFingerprint::tversky;
	break;
    }

  }

  // **************************************************************************
  // calculate the distance between this fingerprint and the one passed in
  // using dist_calc_
  double NotHashedFingerprint::calc_distance( const FingerprintBase &f ) const {

    return f.calc_distance( *this );

  }

  // **************************************************************************
  // calculate the distance between this fingerprint and the one passed in
  // using dist_calc_
  double NotHashedFingerprint::calc_distance( const FingerprintBase &f ,
					      float threshold ) const {

    return f.calc_distance( *this , threshold );

  }

  // **************************************************************************
  // calculate the distance between this fingerprint and the one passed in
  // using dist_calc_
  double NotHashedFingerprint::calc_distance( const HashedFingerprint &f __attribute__((unused)) ) const {

    throw IncompatibleFingerprintError( "calc_distance" );

  }

  // **************************************************************************
  // calculate the distance between this fingerprint and the one passed
  // in using threshold_dist_calc_.  If the distance is predicted to be above
  // the threshold, return 1.0
  double NotHashedFingerprint::calc_distance( const HashedFingerprint &f __attribute__((unused)) ,
                                              float threshold __attribute__((unused)) ) const {

    throw IncompatibleFingerprintError( "calc_distance" );

  }

  // ****************************************************************************
  // calculate the distance between this fingerprint and the one passed in
  // using dist_calc_
  double NotHashedFingerprint::calc_distance( const NotHashedFingerprint &f ) const {

    return (this->*dist_calc_)( f );

  }

  // ****************************************************************************
  // calculate the distance between this fingerprint and the one passed
  // in using threshold_dist_calc_.  If the distance is predicted to be above
  // the threshold, return 1.0
  double NotHashedFingerprint::calc_distance( const NotHashedFingerprint &f ,
					      float threshold ) const {

    return (this->*threshold_dist_calc_)( f , threshold );

  }

  // ****************************************************************************
  void NotHashedFingerprint::copy_data( const NotHashedFingerprint &fp ) {

    FingerprintBase::copy_data( fp );

    delete [] frag_nums_;

    num_frag_nums_ = fp.num_frag_nums_;
    if( fp.frag_nums_ && num_frag_nums_ ) {
      frag_nums_ = new uint32_t[num_frag_nums_];
      copy( fp.frag_nums_ , fp.frag_nums_ + num_frag_nums_ , frag_nums_ );
    } else {
      frag_nums_ = 0;
    }

  }

  // ****************************************************************************
  void NotHashedFingerprint::build_from_vector( const vector<uint32_t> &in_nums ) {

    delete [] frag_nums_;

    vector<uint32_t> tmp( in_nums );
    sort( tmp.begin() , tmp.end() );
    tmp.erase( unique( tmp.begin() , tmp.end() ) , tmp.end() );

    num_frag_nums_ = tmp.size();
    if( num_frag_nums_ ) {
      frag_nums_ = new uint32_t[num_frag_nums_];
      copy( tmp.begin() , tmp.end() , frag_nums_ );
    } else {
      frag_nums_ = 0;
    }

  }

} // end of namespace DAC_FINGERPRINTS


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_E6519754D19211DFB8405F74DFD72085
#define UUID_E6519754D19211DFB8405F74DFD72085

#include <boost/qvm/assert.hpp>
#include <boost/qvm/deduce_quat.hpp>
#include <boost/qvm/detail/quat_assign.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/math.hpp>
#include <boost/qvm/scalar_traits.hpp>
#include <boost/qvm/throw_exception.hpp>
#include <string>

namespace boost {
namespace qvm {
namespace qvm_detail {
BOOST_QVM_INLINE_CRITICAL
void const *get_valid_ptr_quat_operations() {
  static int const obj = 0;
  return &obj;
}
} // namespace qvm_detail

////////////////////////////////////////////////

namespace msvc_parse_bug_workaround {
template <class A, class B> struct quats {
  static bool const value = is_quat<A>::value && is_quat<B>::value;
};
} // namespace msvc_parse_bug_workaround

namespace qvm_to_string_detail {
template <class T> std::string to_string(T const &x);
}

template <class A>
inline typename boost::enable_if_c<is_quat<A>::value, std::string>::type
to_string(A const &a) {
  using namespace qvm_to_string_detail;
  return '(' + to_string(quat_traits<A>::template read_element<0>(a)) + ',' +
         to_string(quat_traits<A>::template read_element<1>(a)) + ',' +
         to_string(quat_traits<A>::template read_element<2>(a)) + ',' +
         to_string(quat_traits<A>::template read_element<3>(a)) + ')';
}

////////////////////////////////////////////////

template <class A, class B, class Cmp>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_quat<B>::value, bool>::type
    cmp(A const &a, B const &b, Cmp f) {
  typedef typename deduce_scalar<typename quat_traits<A>::scalar_type,
                                 typename quat_traits<B>::scalar_type>::type T;
  T q1[4] = {quat_traits<A>::template read_element<0>(a),
             quat_traits<A>::template read_element<1>(a),
             quat_traits<A>::template read_element<2>(a),
             quat_traits<A>::template read_element<3>(a)};
  T q2[4] = {quat_traits<B>::template read_element<0>(b),
             quat_traits<B>::template read_element<1>(b),
             quat_traits<B>::template read_element<2>(b),
             quat_traits<B>::template read_element<3>(b)};
  int i;
  for (i = 0; i != 4; ++i)
    if (!f(q1[i], q2[i]))
      break;
  if (i == 4)
    return true;
  for (i = 0; i != 4; ++i)
    if (!f(q1[i], -q2[i]))
      return false;
  return true;
}

////////////////////////////////////////////////

template <class R, class A>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<R>::value && is_quat<A>::value, R>::type
    convert_to(A const &a) {
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a);
  quat_traits<R>::template write_element<1>(r) =
      quat_traits<A>::template read_element<1>(a);
  quat_traits<R>::template write_element<2>(r) =
      quat_traits<A>::template read_element<2>(a);
  quat_traits<R>::template write_element<3>(r) =
      quat_traits<A>::template read_element<3>(a);
  return r;
}

template <class R, class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<R>::value && is_mat<A>::value &&
                             mat_traits<A>::rows == 3 &&
                             mat_traits<A>::cols == 3,
                         R>::type
    convert_to(A const &a) {
  typedef typename mat_traits<A>::scalar_type T;
  T const mat[3][3] = {{mat_traits<A>::template read_element<0, 0>(a),
                        mat_traits<A>::template read_element<0, 1>(a),
                        mat_traits<A>::template read_element<0, 2>(a)},
                       {mat_traits<A>::template read_element<1, 0>(a),
                        mat_traits<A>::template read_element<1, 1>(a),
                        mat_traits<A>::template read_element<1, 2>(a)},
                       {mat_traits<A>::template read_element<2, 0>(a),
                        mat_traits<A>::template read_element<2, 1>(a),
                        mat_traits<A>::template read_element<2, 2>(a)}};
  R r;
  if (mat[0][0] + mat[1][1] + mat[2][2] > scalar_traits<T>::value(0)) {
    T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
    T s = (scalar_traits<T>::value(1) / sqrt<T>(t)) / 2;
    quat_traits<R>::template write_element<0>(r) = s * t;
    quat_traits<R>::template write_element<1>(r) = (mat[2][1] - mat[1][2]) * s;
    quat_traits<R>::template write_element<2>(r) = (mat[0][2] - mat[2][0]) * s;
    quat_traits<R>::template write_element<3>(r) = (mat[1][0] - mat[0][1]) * s;
  } else if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
    T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
    T s = (scalar_traits<T>::value(1) / sqrt<T>(t)) / 2;
    quat_traits<R>::template write_element<0>(r) = (mat[2][1] - mat[1][2]) * s;
    quat_traits<R>::template write_element<1>(r) = s * t;
    quat_traits<R>::template write_element<2>(r) = (mat[1][0] + mat[0][1]) * s;
    quat_traits<R>::template write_element<3>(r) = (mat[0][2] + mat[2][0]) * s;
  } else if (mat[1][1] > mat[2][2]) {
    T t = -mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
    T s = (scalar_traits<T>::value(1) / sqrt<T>(t)) / 2;
    quat_traits<R>::template write_element<0>(r) = (mat[0][2] - mat[2][0]) * s;
    quat_traits<R>::template write_element<1>(r) = (mat[1][0] + mat[0][1]) * s;
    quat_traits<R>::template write_element<2>(r) = s * t;
    quat_traits<R>::template write_element<3>(r) = (mat[2][1] + mat[1][2]) * s;
  } else {
    T t = -mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
    T s = (scalar_traits<T>::value(1) / sqrt<T>(t)) / 2;
    quat_traits<R>::template write_element<0>(r) = (mat[1][0] - mat[0][1]) * s;
    quat_traits<R>::template write_element<1>(r) = (mat[0][2] + mat[2][0]) * s;
    quat_traits<R>::template write_element<2>(r) = (mat[2][1] + mat[1][2]) * s;
    quat_traits<R>::template write_element<3>(r) = s * t;
  }
  return r;
}

////////////////////////////////////////////////

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value, deduce_quat<A>>::type
    conjugate(A const &a) {
  typedef typename deduce_quat<A>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a);
  quat_traits<R>::template write_element<1>(r) =
      -quat_traits<A>::template read_element<1>(a);
  quat_traits<R>::template write_element<2>(r) =
      -quat_traits<A>::template read_element<2>(a);
  quat_traits<R>::template write_element<3>(r) =
      -quat_traits<A>::template read_element<3>(a);
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> class identity_quat_ {
  identity_quat_(identity_quat_ const &);
  identity_quat_ &operator=(identity_quat_ const &);
  ~identity_quat_();

public:
  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class T> struct quat_traits<qvm_detail::identity_quat_<T>> {
  typedef qvm_detail::identity_quat_<T> this_quaternion;
  typedef T scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return scalar_traits<T>::value(I == 0);
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_quaternion const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < 4);
    return scalar_traits<T>::value(i == 0);
  }
};

template <class T> struct deduce_quat<qvm_detail::identity_quat_<T>> {
  typedef quat<T> type;
};

template <class T>
struct deduce_quat2<qvm_detail::identity_quat_<T>,
                    qvm_detail::identity_quat_<T>> {
  typedef quat<T> type;
};

template <class T>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::identity_quat_<T> const &identity_quat() {
  return *(qvm_detail::identity_quat_<T> const *)
      qvm_detail::get_valid_ptr_quat_operations();
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
set_identity(A &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T const zero = scalar_traits<T>::value(0);
  T const one = scalar_traits<T>::value(1);
  quat_traits<A>::template write_element<0>(a) = one;
  quat_traits<A>::template write_element<1>(a) = zero;
  quat_traits<A>::template write_element<2>(a) = zero;
  quat_traits<A>::template write_element<3>(a) = zero;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class OriginalType, class Scalar> class quaternion_scalar_cast_ {
  quaternion_scalar_cast_(quaternion_scalar_cast_ const &);
  quaternion_scalar_cast_ &operator=(quaternion_scalar_cast_ const &);
  ~quaternion_scalar_cast_();

public:
  template <class T>
  BOOST_QVM_INLINE_TRIVIAL quaternion_scalar_cast_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};

template <bool> struct scalar_cast_quaternion_filter {};
template <> struct scalar_cast_quaternion_filter<true> { typedef int type; };
} // namespace qvm_detail

template <class OriginalType, class Scalar>
struct quat_traits<qvm_detail::quaternion_scalar_cast_<OriginalType, Scalar>> {
  typedef Scalar scalar_type;
  typedef qvm_detail::quaternion_scalar_cast_<OriginalType, Scalar>
      this_quaternion;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return scalar_type(quat_traits<OriginalType>::template read_element<I>(
        reinterpret_cast<OriginalType const &>(x)));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_quaternion const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < 4);
    return scalar_type(quat_traits<OriginalType>::read_element_idx(
        i, reinterpret_cast<OriginalType const &>(x)));
  }
};

template <class Scalar, class T>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::quaternion_scalar_cast_<T, Scalar> const &
scalar_cast(T const &x, typename qvm_detail::scalar_cast_quaternion_filter<
                            is_quat<T>::value>::type = 0) {
  return reinterpret_cast<
      qvm_detail::quaternion_scalar_cast_<T, Scalar> const &>(x);
}

////////////////////////////////////////////////

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_scalar<B>::value, A &>::type
    operator/=(A &a, B b) {
  quat_traits<A>::template write_element<0>(a) /= b;
  quat_traits<A>::template write_element<1>(a) /= b;
  quat_traits<A>::template write_element<2>(a) /= b;
  quat_traits<A>::template write_element<3>(a) /= b;
  return a;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_scalar<B>::value,
                              deduce_quat<A>>::type
    operator/(A const &a, B b) {
  typedef typename deduce_quat<A>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a) / b;
  quat_traits<R>::template write_element<1>(r) =
      quat_traits<A>::template read_element<1>(a) / b;
  quat_traits<R>::template write_element<2>(r) =
      quat_traits<A>::template read_element<2>(a) / b;
  quat_traits<R>::template write_element<3>(r) =
      quat_traits<A>::template read_element<3>(a) / b;
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_quat<A>::value && is_quat<B>::value,
    deduce_scalar<typename quat_traits<A>::scalar_type,
                  typename quat_traits<B>::scalar_type>>::type
dot(A const &a, B const &b) {
  typedef typename quat_traits<A>::scalar_type Ta;
  typedef typename quat_traits<B>::scalar_type Tb;
  typedef typename deduce_scalar<Ta, Tb>::type Tr;
  Ta const a0 = quat_traits<A>::template read_element<0>(a);
  Ta const a1 = quat_traits<A>::template read_element<1>(a);
  Ta const a2 = quat_traits<A>::template read_element<2>(a);
  Ta const a3 = quat_traits<A>::template read_element<3>(a);
  Tb const b0 = quat_traits<B>::template read_element<0>(b);
  Tb const b1 = quat_traits<B>::template read_element<1>(b);
  Tb const b2 = quat_traits<B>::template read_element<2>(b);
  Tb const b3 = quat_traits<B>::template read_element<3>(b);
  Tr const dp = a0 * b0 + a1 * b1 + a2 * b2 + a3 * b3;
  return dp;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_quat<B>::value, bool>::type
    operator==(A const &a, B const &b) {
  return quat_traits<A>::template read_element<0>(a) ==
             quat_traits<B>::template read_element<0>(b) &&
         quat_traits<A>::template read_element<1>(a) ==
             quat_traits<B>::template read_element<1>(b) &&
         quat_traits<A>::template read_element<2>(a) ==
             quat_traits<B>::template read_element<2>(b) &&
         quat_traits<A>::template read_element<3>(a) ==
             quat_traits<B>::template read_element<3>(b);
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value, deduce_quat<A>>::type
    inverse(A const &a) {
  typedef typename deduce_quat<A>::type R;
  typedef typename quat_traits<A>::scalar_type TA;
  TA aa = quat_traits<A>::template read_element<0>(a);
  TA ab = quat_traits<A>::template read_element<1>(a);
  TA ac = quat_traits<A>::template read_element<2>(a);
  TA ad = quat_traits<A>::template read_element<3>(a);
  TA m2 = ab * ab + ac * ac + ad * ad + aa * aa;
  if (m2 == scalar_traits<TA>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  TA rm = scalar_traits<TA>::value(1) / m2;
  R r;
  quat_traits<R>::template write_element<0>(r) = aa * rm;
  quat_traits<R>::template write_element<1>(r) = -ab * rm;
  quat_traits<R>::template write_element<2>(r) = -ac * rm;
  quat_traits<R>::template write_element<3>(r) = -ad * rm;
  return r;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value,
                         typename quat_traits<A>::scalar_type>::type
    mag_sqr(A const &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T x = quat_traits<A>::template read_element<0>(a);
  T y = quat_traits<A>::template read_element<1>(a);
  T z = quat_traits<A>::template read_element<2>(a);
  T w = quat_traits<A>::template read_element<3>(a);
  return x * x + y * y + z * z + w * w;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value,
                         typename quat_traits<A>::scalar_type>::type
    mag(A const &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T x = quat_traits<A>::template read_element<0>(a);
  T y = quat_traits<A>::template read_element<1>(a);
  T z = quat_traits<A>::template read_element<2>(a);
  T w = quat_traits<A>::template read_element<3>(a);
  return sqrt<T>(x * x + y * y + z * z + w * w);
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if<msvc_parse_bug_workaround::quats<A, B>, A &>::type
    operator-=(A &a, B const &b) {
  quat_traits<A>::template write_element<0>(a) -=
      quat_traits<B>::template read_element<0>(b);
  quat_traits<A>::template write_element<1>(a) -=
      quat_traits<B>::template read_element<1>(b);
  quat_traits<A>::template write_element<2>(a) -=
      quat_traits<B>::template read_element<2>(b);
  quat_traits<A>::template write_element<3>(a) -=
      quat_traits<B>::template read_element<3>(b);
  return a;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_quat<B>::value,
                              deduce_quat2<A, B>>::type
    operator-(A const &a, B const &b) {
  typedef typename deduce_quat2<A, B>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a) -
      quat_traits<B>::template read_element<0>(b);
  quat_traits<R>::template write_element<1>(r) =
      quat_traits<A>::template read_element<1>(a) -
      quat_traits<B>::template read_element<1>(b);
  quat_traits<R>::template write_element<2>(r) =
      quat_traits<A>::template read_element<2>(a) -
      quat_traits<B>::template read_element<2>(b);
  quat_traits<R>::template write_element<3>(r) =
      quat_traits<A>::template read_element<3>(a) -
      quat_traits<B>::template read_element<3>(b);
  return r;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value, deduce_quat<A>>::type
    operator-(A const &a) {
  typedef typename deduce_quat<A>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      -quat_traits<A>::template read_element<0>(a);
  quat_traits<R>::template write_element<1>(r) =
      -quat_traits<A>::template read_element<1>(a);
  quat_traits<R>::template write_element<2>(r) =
      -quat_traits<A>::template read_element<2>(a);
  quat_traits<R>::template write_element<3>(r) =
      -quat_traits<A>::template read_element<3>(a);
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if<msvc_parse_bug_workaround::quats<A, B>, A &>::type
    operator*=(A &a, B const &b) {
  typedef typename quat_traits<A>::scalar_type TA;
  typedef typename quat_traits<B>::scalar_type TB;
  TA const aa = quat_traits<A>::template read_element<0>(a);
  TA const ab = quat_traits<A>::template read_element<1>(a);
  TA const ac = quat_traits<A>::template read_element<2>(a);
  TA const ad = quat_traits<A>::template read_element<3>(a);
  TB const ba = quat_traits<B>::template read_element<0>(b);
  TB const bb = quat_traits<B>::template read_element<1>(b);
  TB const bc = quat_traits<B>::template read_element<2>(b);
  TB const bd = quat_traits<B>::template read_element<3>(b);
  quat_traits<A>::template write_element<0>(a) =
      aa * ba - ab * bb - ac * bc - ad * bd;
  quat_traits<A>::template write_element<1>(a) =
      aa * bb + ab * ba + ac * bd - ad * bc;
  quat_traits<A>::template write_element<2>(a) =
      aa * bc + ac * ba + ad * bb - ab * bd;
  quat_traits<A>::template write_element<3>(a) =
      aa * bd + ad * ba + ab * bc - ac * bb;
  return a;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_scalar<B>::value, A &>::type
    operator*=(A &a, B b) {
  quat_traits<A>::template write_element<0>(a) *= b;
  quat_traits<A>::template write_element<1>(a) *= b;
  quat_traits<A>::template write_element<2>(a) *= b;
  quat_traits<A>::template write_element<3>(a) *= b;
  return a;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_quat<B>::value,
                              deduce_quat2<A, B>>::type
    operator*(A const &a, B const &b) {
  typedef typename deduce_quat2<A, B>::type R;
  typedef typename quat_traits<A>::scalar_type TA;
  typedef typename quat_traits<B>::scalar_type TB;
  TA const aa = quat_traits<A>::template read_element<0>(a);
  TA const ab = quat_traits<A>::template read_element<1>(a);
  TA const ac = quat_traits<A>::template read_element<2>(a);
  TA const ad = quat_traits<A>::template read_element<3>(a);
  TB const ba = quat_traits<B>::template read_element<0>(b);
  TB const bb = quat_traits<B>::template read_element<1>(b);
  TB const bc = quat_traits<B>::template read_element<2>(b);
  TB const bd = quat_traits<B>::template read_element<3>(b);
  R r;
  quat_traits<R>::template write_element<0>(r) =
      aa * ba - ab * bb - ac * bc - ad * bd;
  quat_traits<R>::template write_element<1>(r) =
      aa * bb + ab * ba + ac * bd - ad * bc;
  quat_traits<R>::template write_element<2>(r) =
      aa * bc + ac * ba + ad * bb - ab * bd;
  quat_traits<R>::template write_element<3>(r) =
      aa * bd + ad * ba + ab * bc - ac * bb;
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_scalar<B>::value,
                              deduce_quat<A>>::type
    operator*(A const &a, B b) {
  typedef typename deduce_quat<A>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a) * b;
  quat_traits<R>::template write_element<1>(r) =
      quat_traits<A>::template read_element<1>(a) * b;
  quat_traits<R>::template write_element<2>(r) =
      quat_traits<A>::template read_element<2>(a) * b;
  quat_traits<R>::template write_element<3>(r) =
      quat_traits<A>::template read_element<3>(a) * b;
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_quat<B>::value, bool>::type
    operator!=(A const &a, B const &b) {
  return quat_traits<A>::template read_element<0>(a) !=
             quat_traits<B>::template read_element<0>(b) ||
         quat_traits<A>::template read_element<1>(a) !=
             quat_traits<B>::template read_element<1>(b) ||
         quat_traits<A>::template read_element<2>(a) !=
             quat_traits<B>::template read_element<2>(b) ||
         quat_traits<A>::template read_element<3>(a) !=
             quat_traits<B>::template read_element<3>(b);
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value, deduce_quat<A>>::type
    normalized(A const &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T const a0 = quat_traits<A>::template read_element<0>(a);
  T const a1 = quat_traits<A>::template read_element<1>(a);
  T const a2 = quat_traits<A>::template read_element<2>(a);
  T const a3 = quat_traits<A>::template read_element<3>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3;
  if (m2 == scalar_traits<typename quat_traits<A>::scalar_type>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T const rm = scalar_traits<T>::value(1) / sqrt<T>(m2);
  typedef typename deduce_quat<A>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) = a0 * rm;
  quat_traits<R>::template write_element<1>(r) = a1 * rm;
  quat_traits<R>::template write_element<2>(r) = a2 * rm;
  quat_traits<R>::template write_element<3>(r) = a3 * rm;
  return r;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
normalize(A &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T const a0 = quat_traits<A>::template read_element<0>(a);
  T const a1 = quat_traits<A>::template read_element<1>(a);
  T const a2 = quat_traits<A>::template read_element<2>(a);
  T const a3 = quat_traits<A>::template read_element<3>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3;
  if (m2 == scalar_traits<typename quat_traits<A>::scalar_type>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T const rm = scalar_traits<T>::value(1) / sqrt<T>(m2);
  quat_traits<A>::template write_element<0>(a) *= rm;
  quat_traits<A>::template write_element<1>(a) *= rm;
  quat_traits<A>::template write_element<2>(a) *= rm;
  quat_traits<A>::template write_element<3>(a) *= rm;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if<msvc_parse_bug_workaround::quats<A, B>, A &>::type
    operator+=(A &a, B const &b) {
  quat_traits<A>::template write_element<0>(a) +=
      quat_traits<B>::template read_element<0>(b);
  quat_traits<A>::template write_element<1>(a) +=
      quat_traits<B>::template read_element<1>(b);
  quat_traits<A>::template write_element<2>(a) +=
      quat_traits<B>::template read_element<2>(b);
  quat_traits<A>::template write_element<3>(a) +=
      quat_traits<B>::template read_element<3>(b);
  return a;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_quat<B>::value,
                              deduce_quat2<A, B>>::type
    operator+(A const &a, B const &b) {
  typedef typename deduce_quat2<A, B>::type R;
  R r;
  quat_traits<R>::template write_element<0>(r) =
      quat_traits<A>::template read_element<0>(a) +
      quat_traits<B>::template read_element<0>(b);
  quat_traits<R>::template write_element<1>(r) =
      quat_traits<A>::template read_element<1>(a) +
      quat_traits<B>::template read_element<1>(b);
  quat_traits<R>::template write_element<2>(r) =
      quat_traits<A>::template read_element<2>(a) +
      quat_traits<B>::template read_element<2>(b);
  quat_traits<R>::template write_element<3>(r) =
      quat_traits<A>::template read_element<3>(a) +
      quat_traits<B>::template read_element<3>(b);
  return r;
}

template <class A, class B, class C>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_quat<B>::value &&
                                  is_scalar<C>::value,
                              deduce_quat2<A, B>>::type
    slerp(A const &a, B const &b, C t) {
  typedef typename deduce_quat2<A, B>::type R;
  typedef typename quat_traits<R>::scalar_type TR;
  TR const one = scalar_traits<TR>::value(1);
  TR dp = dot(a, b);
  TR sc = one;
  if (dp < one) {
    TR const theta = acos<TR>(dp);
    TR const invsintheta = one / sin<TR>(theta);
    TR const scale = sin<TR>(theta * (one - t)) * invsintheta;
    TR const invscale = sin<TR>(theta * t) * invsintheta * sc;
    return a * scale + b * invscale;
  } else
    return normalized(a + (b - a) * t);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> class qref_ {
  qref_(qref_ const &);
  qref_ &operator=(qref_ const &);
  ~qref_();

public:
  template <class R> BOOST_QVM_INLINE_TRIVIAL qref_ &operator=(R const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class Q> struct quat_traits;

template <class Q> struct quat_traits<qvm_detail::qref_<Q>> {
  typedef typename quat_traits<Q>::scalar_type scalar_type;
  typedef qvm_detail::qref_<Q> this_quaternion;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return quat_traits<Q>::template read_element<I>(
        reinterpret_cast<Q const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element(this_quaternion &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x));
  }
};

template <class Q> struct deduce_quat<qvm_detail::qref_<Q>> {
  typedef quat<typename quat_traits<Q>::scalar_type> type;
};

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value, qvm_detail::qref_<Q> const &>::type
    qref(Q const &a) {
  return reinterpret_cast<qvm_detail::qref_<Q> const &>(a);
}

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value, qvm_detail::qref_<Q> &>::type
    qref(Q &a) {
  return reinterpret_cast<qvm_detail::qref_<Q> &>(a);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> class zero_q_ {
  zero_q_(zero_q_ const &);
  zero_q_ &operator=(zero_q_ const &);
  ~zero_q_();

public:
  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class T> struct quat_traits<qvm_detail::zero_q_<T>> {
  typedef qvm_detail::zero_q_<T> this_quaternion;
  typedef T scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return scalar_traits<scalar_type>::value(0);
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_quaternion const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < 4);
    return scalar_traits<scalar_type>::value(0);
  }
};

template <class T>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::zero_q_<T> const &zero_quat() {
  return *(qvm_detail::zero_q_<T> const *)
      qvm_detail::get_valid_ptr_quat_operations();
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
set_zero(A &a) {
  typedef typename quat_traits<A>::scalar_type T;
  T const zero = scalar_traits<T>::value(0);
  quat_traits<A>::template write_element<0>(a) = zero;
  quat_traits<A>::template write_element<1>(a) = zero;
  quat_traits<A>::template write_element<2>(a) = zero;
  quat_traits<A>::template write_element<3>(a) = zero;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class V> struct rot_quat_ {
  typedef typename vec_traits<V>::scalar_type scalar_type;
  scalar_type a[4];

  template <class Angle>
  BOOST_QVM_INLINE rot_quat_(V const &axis, Angle angle) {
    scalar_type const x = vec_traits<V>::template read_element<0>(axis);
    scalar_type const y = vec_traits<V>::template read_element<1>(axis);
    scalar_type const z = vec_traits<V>::template read_element<2>(axis);
    scalar_type const m2 = x * x + y * y + z * z;
    if (m2 == scalar_traits<scalar_type>::value(0))
      BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
    scalar_type const rm =
        scalar_traits<scalar_type>::value(1) / sqrt<scalar_type>(m2);
    angle /= 2;
    scalar_type const s = sin<Angle>(angle);
    a[0] = cos<Angle>(angle);
    a[1] = rm * x * s;
    a[2] = rm * y * s;
    a[3] = rm * z * s;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class V> struct quat_traits<qvm_detail::rot_quat_<V>> {
  typedef qvm_detail::rot_quat_<V> this_quaternion;
  typedef typename this_quaternion::scalar_type scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return x.a[I];
  }
};

template <class V> struct deduce_quat<qvm_detail::rot_quat_<V>> {
  typedef quat<typename vec_traits<V>::scalar_type> type;
};

template <class A, class Angle>
BOOST_QVM_INLINE
    typename enable_if_c<is_vec<A>::value && vec_traits<A>::dim == 3,
                         qvm_detail::rot_quat_<A>>::type
    rot_quat(A const &axis, Angle angle) {
  return qvm_detail::rot_quat_<A>(axis, angle);
}

template <class A, class B, class Angle>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_vec<B>::value &&
                             vec_traits<B>::dim == 3,
                         void>::type
    set_rot(A &a, B const &axis, Angle angle) {
  assign(a, rot_quat(axis, angle));
}

template <class A, class B, class Angle>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_vec<B>::value &&
                             vec_traits<B>::dim == 3,
                         void>::type
    rotate(A &a, B const &axis, Angle angle) {
  a *= rot_quat(axis, angle);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> struct rotx_quat_ {
  BOOST_QVM_INLINE_TRIVIAL
  rotx_quat_() {}

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }

private:
  rotx_quat_(rotx_quat_ const &);
  rotx_quat_ &operator=(rotx_quat_ const &);
  ~rotx_quat_();
};

template <int I> struct rotx_q_get {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &) {
    return scalar_traits<T>::value(0);
  }
};

template <> struct rotx_q_get<1> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return sin<T>(angle / 2);
  }
};

template <> struct rotx_q_get<0> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return cos<T>(angle / 2);
  }
};
} // namespace qvm_detail

template <class Angle> struct quat_traits<qvm_detail::rotx_quat_<Angle>> {
  typedef qvm_detail::rotx_quat_<Angle> this_quaternion;
  typedef Angle scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x));
  }
};

template <class Angle> struct deduce_quat<qvm_detail::rotx_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
struct deduce_quat2<qvm_detail::rotx_quat_<Angle>,
                    qvm_detail::rotx_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::rotx_quat_<Angle> const &
rotx_quat(Angle const &angle) {
  return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle);
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
set_rotx(A &a, Angle angle) {
  assign(a, rotx_quat(angle));
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
rotate_x(A &a, Angle angle) {
  a *= rotx_quat(angle);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> struct roty_quat_ {
  BOOST_QVM_INLINE_TRIVIAL
  roty_quat_() {}

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }

private:
  roty_quat_(roty_quat_ const &);
  roty_quat_ &operator=(roty_quat_ const &);
  ~roty_quat_();
};

template <int I> struct roty_q_get {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &) {
    return scalar_traits<T>::value(0);
  }
};

template <> struct roty_q_get<2> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return sin<T>(angle / 2);
  }
};

template <> struct roty_q_get<0> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return cos<T>(angle / 2);
  }
};
} // namespace qvm_detail

template <class Angle> struct quat_traits<qvm_detail::roty_quat_<Angle>> {
  typedef qvm_detail::roty_quat_<Angle> this_quaternion;
  typedef Angle scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x));
  }
};

template <class Angle> struct deduce_quat<qvm_detail::roty_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
struct deduce_quat2<qvm_detail::roty_quat_<Angle>,
                    qvm_detail::roty_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::roty_quat_<Angle> const &
roty_quat(Angle const &angle) {
  return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle);
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
set_roty(A &a, Angle angle) {
  assign(a, roty_quat(angle));
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
rotate_y(A &a, Angle angle) {
  a *= roty_quat(angle);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> struct rotz_quat_ {
  BOOST_QVM_INLINE_TRIVIAL
  rotz_quat_() {}

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }

private:
  rotz_quat_(rotz_quat_ const &);
  rotz_quat_ &operator=(rotz_quat_ const &);
  ~rotz_quat_();
};

template <int I> struct rotz_q_get {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &) {
    return scalar_traits<T>::value(0);
  }
};

template <> struct rotz_q_get<3> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return sin<T>(angle / 2);
  }
};

template <> struct rotz_q_get<0> {
  template <class T> static BOOST_QVM_INLINE_CRITICAL T get(T const &angle) {
    return cos<T>(angle / 2);
  }
};
} // namespace qvm_detail

template <class Angle> struct quat_traits<qvm_detail::rotz_quat_<Angle>> {
  typedef qvm_detail::rotz_quat_<Angle> this_quaternion;
  typedef Angle scalar_type;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_quaternion const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < 4);
    return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x));
  }
};

template <class Angle> struct deduce_quat<qvm_detail::rotz_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
struct deduce_quat2<qvm_detail::rotz_quat_<Angle>,
                    qvm_detail::rotz_quat_<Angle>> {
  typedef quat<Angle> type;
};

template <class Angle>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::rotz_quat_<Angle> const &
rotz_quat(Angle const &angle) {
  return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle);
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
set_rotz(A &a, Angle angle) {
  assign(a, rotz_quat(angle));
}

template <class A, class Angle>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_quat<A>::value, void>::type
rotate_z(A &a, Angle angle) {
  a *= rotz_quat(angle);
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_quat<A>::value && is_vec<B>::value &&
                             vec_traits<B>::dim == 3,
                         typename quat_traits<A>::scalar_type>::type
    axis_angle(A const &a, B &b) {
  typedef typename quat_traits<A>::scalar_type T;
  T a0 = quat_traits<A>::template read_element<0>(a);
  T a1 = quat_traits<A>::template read_element<1>(a);
  T a2 = quat_traits<A>::template read_element<2>(a);
  T a3 = quat_traits<A>::template read_element<3>(a);
  if (a0 > 1) {
    T const m2 = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3;
    if (m2 == scalar_traits<T>::value(0))
      BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
    T const s = sqrt<T>(m2);
    a0 /= s;
    a1 /= s;
    a2 /= s;
    a3 /= s;
  }
  if (T s = sqrt<T>(1 - a0 * a0)) {
    vec_traits<B>::template write_element<0>(b) = a1 / s;
    vec_traits<B>::template write_element<1>(b) = a2 / s;
    vec_traits<B>::template write_element<2>(b) = a3 / s;
  } else {
    typedef typename vec_traits<B>::scalar_type U;
    vec_traits<B>::template write_element<0>(b) = scalar_traits<U>::value(1);
    vec_traits<B>::template write_element<1>(b) =
        vec_traits<B>::template write_element<2>(b) =
            scalar_traits<U>::value(0);
  }
  return scalar_traits<T>::value(2) * qvm::acos(a0);
}

////////////////////////////////////////////////

namespace sfinae {
using ::boost::qvm::assign;
using ::boost::qvm::cmp;
using ::boost::qvm::conjugate;
using ::boost::qvm::convert_to;
using ::boost::qvm::scalar_cast;
using ::boost::qvm::set_identity;
using ::boost::qvm::set_zero;
using ::boost::qvm::operator/=;
using ::boost::qvm::operator/;
using ::boost::qvm::dot;
using ::boost::qvm::operator==;
using ::boost::qvm::inverse;
using ::boost::qvm::mag;
using ::boost::qvm::mag_sqr;
using ::boost::qvm::slerp;
using ::boost::qvm::operator-=;
using ::boost::qvm::operator-;
using ::boost::qvm::operator*=;
using ::boost::qvm::operator*;
using ::boost::qvm::operator!=;
using ::boost::qvm::normalize;
using ::boost::qvm::normalized;
using ::boost::qvm::operator+=;
using ::boost::qvm::operator+;
using ::boost::qvm::qref;
using ::boost::qvm::rot_quat;
using ::boost::qvm::rotate;
using ::boost::qvm::rotate_x;
using ::boost::qvm::rotate_y;
using ::boost::qvm::rotate_z;
using ::boost::qvm::rotx_quat;
using ::boost::qvm::roty_quat;
using ::boost::qvm::rotz_quat;
using ::boost::qvm::set_rot;
using ::boost::qvm::set_rotx;
using ::boost::qvm::set_roty;
using ::boost::qvm::set_rotz;
} // namespace sfinae

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/* Copyright (c) 2016, Jakob Engel
 * 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. Neither the name of the copyright holder 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 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 COPYRIGHT HOLDER 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.
 */

#include "FOVUndistorter.h"

#include <Eigen/Core>
#include <fstream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <sstream>

UndistorterFOV::UndistorterFOV() {
  remapX = nullptr;
  remapY = nullptr;
  valid = false;
}

UndistorterFOV::UndistorterFOV(const char* configFileName) {
  remapX = nullptr;
  remapY = nullptr;
  valid = false;

  // read parameters
  std::ifstream infile(configFileName);
  if (!infile.good()) {
    printf(
        "Failed to read camera calibration (invalid format?)\nCalibration "
        "file: %s\n",
        configFileName);
    return;
  }

  std::string l1, l2, l3, l4;
  std::getline(infile, l1);
  std::getline(infile, l2);
  std::getline(infile, l3);
  std::getline(infile, l4);

  // l1 & l2
  if (std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0],
                  &inputCalibration[1], &inputCalibration[2],
                  &inputCalibration[3], &inputCalibration[4]) == 5 &&
      std::sscanf(l2.c_str(), "%d %d", &in_width, &in_height) == 2) {
    printf("Input resolution: %d %d\n", in_width, in_height);
    printf("Input Calibration (fx fy cx cy): %f %f %f %f %f\n",
           in_width * inputCalibration[0], in_height * inputCalibration[1],
           in_width * inputCalibration[2], in_height * inputCalibration[3],
           inputCalibration[4]);
  } else {
    printf(
        "Failed to read camera calibration (invalid format?)\nCalibration "
        "file: %s\n",
        configFileName);
    return;
  }

  // l3
  if (l3 == "crop") {
    outputCalibration[0] = -1;
    printf("Out: Crop\n");
  } else if (l3 == "full") {
    outputCalibration[0] = -2;
    printf("Out: Full\n");
  } else if (l3 == "none") {
    printf("NO RECTIFICATION\n");
    return;
  } else if (std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0],
                         &outputCalibration[1], &outputCalibration[2],
                         &outputCalibration[3], &outputCalibration[4]) == 5) {
    printf("Out: %f %f %f %f %f\n", outputCalibration[0], outputCalibration[1],
           outputCalibration[2], outputCalibration[3], outputCalibration[4]);
  } else {
    printf("Out: Failed to Read Output pars... not rectifying.\n");
    return;
  }

  // l4
  if (std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) {
    printf("Output resolution: %d %d\n", out_width, out_height);
  } else {
    printf("Out: Failed to Read Output resolution... not rectifying.\n");
    return;
  }

  valid = true;

  // =============================== find optimal new camera matrix
  // =============================== prep warp matrices
  float dist = inputCalibration[4];
  float d2t = 2.0f * tan(dist / 2.0f);

  // current camera parameters
  float fx = inputCalibration[0] * in_width;
  float fy = inputCalibration[1] * in_height;
  float cx = inputCalibration[2] * in_width - 0.5;
  float cy = inputCalibration[3] * in_height - 0.5;

  // output camera parameters
  float ofx, ofy, ocx, ocy;

  // find new camera matrix for "crop" and "full"
  if (inputCalibration[4] == 0) {
    ofx = inputCalibration[0] * out_width;
    ofy = inputCalibration[1] * out_height;
    ocx = (inputCalibration[2] * out_width) - 0.5;
    ocy = (inputCalibration[3] * out_height) - 0.5;
  } else if (outputCalibration[0] == -1)  // "crop"
  {
    // find left-most and right-most radius
    float left_radius = (cx) / fx;
    float right_radius = (in_width - 1 - cx) / fx;
    float top_radius = (cy) / fy;
    float bottom_radius = (in_height - 1 - cy) / fy;

    float trans_left_radius = tan(left_radius * dist) / d2t;
    float trans_right_radius = tan(right_radius * dist) / d2t;
    float trans_top_radius = tan(top_radius * dist) / d2t;
    float trans_bottom_radius = tan(bottom_radius * dist) / d2t;

    ofy = fy *
          ((top_radius + bottom_radius) /
           (trans_top_radius + trans_bottom_radius)) *
          ((float)out_height / (float)in_height);
    ocy = (trans_top_radius / top_radius) * ofy * cy / fy;

    ofx = fx *
          ((left_radius + right_radius) /
           (trans_left_radius + trans_right_radius)) *
          ((float)out_width / (float)in_width);
    ocx = (trans_left_radius / left_radius) * ofx * cx / fx;

    printf("new K: %f %f %f %f\n", ofx, ofy, ocx, ocy);
    printf("old K: %f %f %f %f\n", fx, fy, cx, cy);
  } else if (outputCalibration[0] == -2)  // "full"
  {
    float left_radius = cx / fx;
    float right_radius = (in_width - 1 - cx) / fx;
    float top_radius = cy / fy;
    float bottom_radius = (in_height - 1 - cy) / fy;

    // find left-most and right-most radius
    float tl_radius = sqrt(left_radius * left_radius + top_radius * top_radius);
    float tr_radius =
        sqrt(right_radius * right_radius + top_radius * top_radius);
    float bl_radius =
        sqrt(left_radius * left_radius + bottom_radius * bottom_radius);
    float br_radius =
        sqrt(right_radius * right_radius + bottom_radius * bottom_radius);

    float trans_tl_radius = tan(tl_radius * dist) / d2t;
    float trans_tr_radius = tan(tr_radius * dist) / d2t;
    float trans_bl_radius = tan(bl_radius * dist) / d2t;
    float trans_br_radius = tan(br_radius * dist) / d2t;

    float hor = std::max(br_radius, tr_radius) + std::max(bl_radius, tl_radius);
    float vert =
        std::max(tr_radius, tl_radius) + std::max(bl_radius, br_radius);

    float trans_hor = std::max(trans_br_radius, trans_tr_radius) +
                      std::max(trans_bl_radius, trans_tl_radius);
    float trans_vert = std::max(trans_tr_radius, trans_tl_radius) +
                       std::max(trans_bl_radius, trans_br_radius);

    ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height);
    ocy = std::max(trans_tl_radius / tl_radius, trans_tr_radius / tr_radius) *
          ofy * cy / fy;

    ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width);
    ocx = std::max(trans_bl_radius / bl_radius, trans_tl_radius / tl_radius) *
          ofx * cx / fx;

    printf("new K: %f %f %f %f\n", ofx, ofy, ocx, ocy);
    printf("old K: %f %f %f %f\n", fx, fy, cx, cy);
  } else {
    ofx = outputCalibration[0] * out_width;
    ofy = outputCalibration[1] * out_height;
    ocx = outputCalibration[2] * out_width - 0.5;
    ocy = outputCalibration[3] * out_height - 0.5;
  }

  outputCalibration[0] = ofx / out_width;
  outputCalibration[1] = ofy / out_height;
  outputCalibration[2] = (ocx + 0.5) / out_width;
  outputCalibration[3] = (ocy + 0.5) / out_height;
  outputCalibration[4] = 0;

  // =============================== build rectification map
  // ===============================
  remapX = new float[out_width * out_height];
  remapY = new float[out_width * out_height];
  for (int y = 0; y < out_height; y++)
    for (int x = 0; x < out_width; x++) {
      remapX[x + y * out_width] = x;
      remapY[x + y * out_width] = y;
    }
  distortCoordinates(remapX, remapY, out_height * out_width);

  bool hasBlackPoints = false;
  for (int i = 0; i < out_width * out_height; i++) {
    if (remapX[i] == 0) remapX[i] = 0.01;
    if (remapY[i] == 0) remapY[i] = 0.01;
    if (remapX[i] == in_width - 1) remapX[i] = in_width - 1.01;
    if (remapY[i] == in_height - 1) remapY[i] = in_height - 1.01;

    if (!(remapX[i] > 0 && remapY[i] > 0 && remapX[i] < in_width - 1 &&
          remapY[i] < in_height - 1)) {
      // printf("black pixel at %d %d %f %f!\n", i, i, remapX[i], remapY[i]);
      hasBlackPoints = true;
      remapX[i] = -1;
      remapY[i] = -1;
    }
  }

  if (hasBlackPoints)
    printf("\n\nFOV Undistorter: Warning! Image has black pixels.\n\n\n");

  // =============================== set Krect ===============================
  Krect.setIdentity();
  Krect(0, 0) = outputCalibration[0] * out_width;
  Krect(1, 1) = outputCalibration[1] * out_height;
  Krect(0, 2) = outputCalibration[2] * out_width - 0.5;
  Krect(1, 2) = outputCalibration[3] * out_height - 0.5;

  Korg.setIdentity();
  Korg(0, 0) = inputCalibration[0] * in_width;
  Korg(1, 1) = inputCalibration[1] * in_height;
  Korg(0, 2) = inputCalibration[2] * in_width - 0.5;
  Korg(1, 2) = inputCalibration[3] * in_height - 0.5;
}

UndistorterFOV::~UndistorterFOV() {
  if (remapX != 0) delete[] remapX;
  if (remapY != 0) delete[] remapY;
}

void UndistorterFOV::distortCoordinates(float* in_x, float* in_y, int n) {
  if (!valid) {
    printf("ERROR: invalid UndistorterFOV!\n");
    return;
  }

  float dist = inputCalibration[4];
  float d2t = 2.0f * tan(dist / 2.0f);

  // current camera parameters
  float fx = inputCalibration[0] * in_width;
  float fy = inputCalibration[1] * in_height;
  float cx = inputCalibration[2] * in_width - 0.5;
  float cy = inputCalibration[3] * in_height - 0.5;

  float ofx = outputCalibration[0] * out_width;
  float ofy = outputCalibration[1] * out_height;
  float ocx = outputCalibration[2] * out_width - 0.5f;
  float ocy = outputCalibration[3] * out_height - 0.5f;

  for (int i = 0; i < n; i++) {
    float x = in_x[i];
    float y = in_y[i];
    float ix = (x - ocx) / ofx;
    float iy = (y - ocy) / ofy;

    float r = sqrtf(ix * ix + iy * iy);
    float fac = (r == 0 || dist == 0) ? 1 : atanf(r * d2t) / (dist * r);

    ix = fx * fac * ix + cx;
    iy = fy * fac * iy + cy;

    in_x[i] = ix;
    in_y[i] = iy;
  }
}

template <typename T>
void UndistorterFOV::undistort(const T* input, float* output, int nPixIn,
                               int nPixOut) const {
  if (!valid) return;

  if (nPixIn != in_width * in_height) {
    printf(
        "ERROR: undistort called with wrong input image dismesions (expected "
        "%d pixel, got %d pixel)\n",
        in_width * in_height, nPixIn);
    return;
  }
  if (nPixOut != out_width * out_height) {
    printf(
        "ERROR: undistort called with wrong output image dismesions (expected "
        "%d pixel, got %d pixel)\n",
        out_width * out_height, nPixOut);
    return;
  }

  for (int idx = 0; idx < out_width * out_height; idx++) {
    // get interp. values
    float xx = remapX[idx];
    float yy = remapY[idx];

    if (xx < 0)
      output[idx] = 0;
    else {
      // get integer and rational parts
      int xxi = xx;
      int yyi = yy;
      xx -= xxi;
      yy -= yyi;
      float xxyy = xx * yy;

      // get array base pointer
      const T* src = input + xxi + yyi * in_width;

      // interpolate (bilinear)
      output[idx] = xxyy * src[1 + in_width] + (yy - xxyy) * src[in_width] +
                    (xx - xxyy) * src[1] + (1 - xx - yy + xxyy) * src[0];
    }
  }
}
template void UndistorterFOV::undistort<float>(const float* input,
                                               float* output, int nPixIn,
                                               int nPixOut) const;
template void UndistorterFOV::undistort<unsigned char>(
    const unsigned char* input, float* output, int nPixIn, int nPixOut) const;