/**
 * @author Manuel Guenther <manuel.guenther@idiap.ch>
 * @date Thu Jun  5 17:47:55 CEST 2014
 *
 * @brief The C++ implementations of the Gabor jet similarities and disparity computation
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.ip.gabor/Similarity.h>
#include <boost/assign.hpp>

static const std::map<bob::ip::gabor::Similarity::SimilarityType, std::string> type_map = boost::assign::map_list_of
  (bob::ip::gabor::Similarity::SCALAR_PRODUCT, "ScalarProduct")
  (bob::ip::gabor::Similarity::CANBERRA, "Canberra")
  (bob::ip::gabor::Similarity::ABS_PHASE, "AbsPhase")
  (bob::ip::gabor::Similarity::DISPARITY, "Disparity")
  (bob::ip::gabor::Similarity::PHASE_DIFF, "PhaseDiff")
  (bob::ip::gabor::Similarity::PHASE_DIFF_PLUS_CANBERRA, "PhaseDiffPlusCanberra")
  ;

const std::string& bob::ip::gabor::Similarity::type_to_name(bob::ip::gabor::Similarity::SimilarityType type){
  return type_map.find(type)->second;
}

bob::ip::gabor::Similarity::SimilarityType bob::ip::gabor::Similarity::name_to_type(const std::string& type){
  for (auto it = type_map.begin(); it != type_map.end(); ++it)
    if (it->second == type)
      return it->first;
  throw std::runtime_error("The given similarity name '" + type + "' does not name an appropriate similarity function type.");
}

bob::ip::gabor::Similarity::Similarity(SimilarityType type, boost::shared_ptr<Transform> gwt)
:
  m_type(type),
  m_gwt(gwt),
  m_disparity(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN())
{
  // initialize, when required
  if (m_type >= DISPARITY){
    if (!m_gwt)
      throw std::runtime_error("The given similarity function type '" + type_to_name(m_type) + "' required to specify the Gabor wavelet transform!");
    init();
  }
}

bob::ip::gabor::Similarity::Similarity(bob::io::base::HDF5File& file)
{
  // load configuration from file
  load(file);
}

static double sqr(double x){return x*x;}

void bob::ip::gabor::Similarity::init(){
  m_confidences.resize(m_gwt->numberOfWavelets());
  m_confidences = 0.;
  m_phase_differences.resize(m_gwt->numberOfWavelets());
  m_phase_differences = 0.;
}

double bob::ip::gabor::Similarity::similarity(const Jet& jet1, const Jet& jet2) const{
  // compute the disparity, if required
  if (m_type < DISPARITY){
    switch (m_type){
      case SCALAR_PRODUCT:
        // normalized scalar product (we assume normalized Gabor jets here!)
        return blitz::dot(jet1.abs(), jet2.abs());
      case CANBERRA:{
        // Canberra similarity
        double sim = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        int size = jet1.length();
        for (int j = 0; j < size; ++j){
          sim += 1. - std::abs(a1(j) - a2(j)) / (a1(j) + a2(j));
        }
        return sim / size;
      }
      case ABS_PHASE:{
        // similarity with absloute values and cosine of phase differences
        double sim = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        const auto& p1 = jet1.phase(),& p2 = jet2.phase();
        int size = jet1.length();
        for (int j = 0; j < size; ++j){
          sim += a1(j) * a2(j) * cos(p1(j) - p2(j));
        }
        return sim;
      }
      default:
        throw std::runtime_error("This should not have happened. Please assure that newly generated Gabor jet similarity functions are actually implemented!");
    }

  } else {
    // here only the disparity-related functions should be computed
    // compute disparity
    disparity(jet1, jet2);

    const std::vector<blitz::TinyVector<double,2> >& kernels = m_gwt->waveletFrequencies();

    switch (m_type){
      case DISPARITY:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        for (int j = 0; j < m_confidences.extent(0); ++j){
          sum += m_confidences(j) * cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
        }
        return sum;
      } // DISPARITY

      case PHASE_DIFF:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        for (int j = 0; j < m_phase_differences.extent(0); ++j){
          sum += cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
        }
        return sum / jet1.length();
      } // PHASE_DIFF

      case PHASE_DIFF_PLUS_CANBERRA:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        for (int j = 0; j < m_phase_differences.extent(0); ++j){
          // add disparity term
          sum += cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
          // add Canberra term
          sum += 1. - std::abs(a1(j) - a2(j)) / (a1(j) + a2(j));
        }
        return sum / (2. * jet1.length());
      }

      default:
        // this should never happen
        throw std::runtime_error("This should not have happened. Please check the implementation of the similarity() functions.");
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////  Disparity estimation  /////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
blitz::TinyVector<double,2> bob::ip::gabor::Similarity::disparity(const Jet& jet1, const Jet& jet2) const{

  // Here, only the disparity based similarity functions are executed
  bob::core::array::assertCZeroBaseContiguous(jet1.jet());
  bob::core::array::assertCZeroBaseContiguous(jet2.jet());
  bob::core::array::assertSameShape(jet1.jet(),jet2.jet());

  // compute confidence vectors
  compute_confidences(jet1, jet2);

  // now, compute the disparity
  compute_disparity();

  // return the disparity
  return m_disparity;
}

static double adjustPhase(double phase){
  return phase - (2.*M_PI)*round(phase / (2.*M_PI));
}

void bob::ip::gabor::Similarity::shift_phase(const Jet& jet, const Jet& reference, Jet& shifted) const{
  bob::core::array::assertSameShape(jet.jet(),reference.jet());
  bob::core::array::assertSameShape(jet.jet(),shifted.jet());

  // compute disparity between jet and reference jet
  disparity(jet, reference);

  // compute phase shift for each jet entry based on disparity vector
  const std::vector<blitz::TinyVector<double,2>>& kernels = m_gwt->waveletFrequencies();
  auto& data = shifted.jet();
  // copy data from original jet
  data = jet.jet();
  // shift phases according to the computed disparity
  for (int j = 0; j < m_phase_differences.extent(0); ++j){
    data(1,j) = adjustPhase(data(1,j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
  }
}

void bob::ip::gabor::Similarity::compute_confidences(const Jet& jet1, const Jet& jet2) const{
  if (m_type < DISPARITY){
    throw std::runtime_error("The disparity computation is not supported for similarity type " + type());
  }
  if (jet1.length() != m_confidences.extent(0)){
    throw std::runtime_error((boost::format("The size of the Gabor jet (%d) and the number of wavelets in the Gabor wavelet transform (%d) differ!") % jet1.length() % m_confidences.extent(0)).str());
  }
  // first, fill confidence and phase difference vectors
  const auto& a1 = jet1.abs(),& a2 = jet2.abs(),& p1 = jet1.phase(),& p2 = jet2.phase();
  for (int j = 0; j < m_confidences.extent(0); ++j){
    m_confidences(j) = a1(j) * a2(j);
    m_phase_differences(j) = adjustPhase(p1(j) - p2(j));
  }
}

void bob::ip::gabor::Similarity::compute_disparity() const{
  // approximate the disparity from the phase differences
  double gamma_x_x = 0., gamma_x_y = 0., gamma_y_y = 0., phi_x = 0., phi_y = 0.;
  // initialize the disparity with 0
  m_disparity = 0.;

  const std::vector<blitz::TinyVector<double,2> >& kernels = m_gwt->waveletFrequencies();
  // iterate backwards through the vector to start with the lowest frequency wavelets
  for (int j = m_confidences.extent(0)-1, level = m_gwt->numberOfScales()-1; level >= 0; --level){
    for (int direction = m_gwt->numberOfDirections()-1; direction >= 0; --direction, --j){
      double
          kjx = kernels[j][1],
          kjy = kernels[j][0],
          conf = m_confidences(j),
          diff = m_phase_differences(j);

      // totalize gamma matrix
      gamma_x_x += kjx * kjx * conf;
      gamma_x_y += kjx * kjy * conf;
      gamma_y_y += kjy * kjy * conf;

      // totalize phi vector
      // estimate the number of cycles that we are off
      double nL = round((diff - m_disparity[1] * kjx - m_disparity[0] * kjy) / (2.*M_PI));
      // totalize corrected phi vector elements
      phi_x += (diff - nL * 2. * M_PI) * conf * kjx;
      phi_y += (diff - nL * 2. * M_PI) * conf * kjy;
    } // for direction

    // re-calculate disparity as d=\Gamma^{-1}\Phi of the (low frequency) wavelet scales that we used up to now
    double gamma_det = gamma_x_x * gamma_y_y - sqr(gamma_x_y);
    m_disparity[1] = (gamma_y_y * phi_x - gamma_x_y * phi_y) / gamma_det;
    m_disparity[0] = (gamma_x_x * phi_y - gamma_x_y * phi_x) / gamma_det;
  } // for level
}

void bob::ip::gabor::Similarity::save(bob::io::base::HDF5File& file) const{

  file.set("Type", type_to_name(m_type));
  if (m_type >= DISPARITY){
    file.createGroup("Transform");
    file.cd("Transform");
    m_gwt->save(file);
    file.cd("..");
  }
}

void bob::ip::gabor::Similarity::load(bob::io::base::HDF5File& file){
  // read value
  m_type = name_to_type(file.read<std::string>("Type"));

  if (m_type >= DISPARITY){
    file.cd("Transform");
    m_gwt.reset(new Transform(file));
    file.cd("..");

    init();
  }
}


/**
 * Copyright 2020 Monchack Audio
 *
 * 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 <iostream>

#include <Windows.h>

// reomve comment out below to use Boost
//#include <boost/multiprecision/cpp_dec_float.hpp>

// Tap size; change this number if necessary. Must be an odd number
#define TAP_SIZE 16383

// remove comment out below to enable high precision mode
//#define HIGH_PRECISION 1

#define DATA_UNIT_SIZE (1024 * 1024)

// 16(15+1)bit  X  scale: 48(47+1)bit =  63(62+1)bit -> 32bit (31bit shift)
#define COEFF_SCALE 47
#define SCALE_SHIFT 31

#if !defined(BOOST_VERSION)

void createHannCoeff(int tapNum, long long* dest, double* dest2)
{
	int coeffNum = (tapNum + 1) / 2;
	double* coeff1 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double* coeff2 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double* coeff3 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double pi = 3.141592653589793;

	coeff1[0] = 2.0f * (22050.0f / 352800.0f);
	for (int i = 1; i < coeffNum; ++i)
	{
		double x = i * 2.0f * pi * (22050.0f / 352800.0f);
		coeff1[i] = sin(x) / (pi * i);
	}

	for (int i = 0; i < coeffNum; ++i)
	{
		double x = 2.0f * pi * i / (double)(tapNum - 1);
		coeff2[i] = 0.5f + 0.5f * cos(x);
	}
	coeff2[coeffNum - 1] = 0;

	long long scale = 1LL << (COEFF_SCALE + 3);

	for (int i = 0; i < coeffNum; ++i)
	{
		coeff3[i] = coeff1[i] * coeff2[i] * scale;
	}

	dest[coeffNum - 1] = (long long)round(coeff3[0]);
	dest2[coeffNum - 1] = (double)(coeff3[0] - round(coeff3[0]));
	for (int i = 1; i < coeffNum; ++i)
	{
		dest[coeffNum - 1 + i] = (long long)round(coeff3[i]);
		dest[coeffNum - 1 - i] = (long long)round(coeff3[i]);
		dest2[coeffNum - 1 + i] = (double)(coeff3[i] - round(coeff3[i]));
		dest2[coeffNum - 1 - i] = (double)(coeff3[i] - round(coeff3[i]));
	}
	::GlobalFree(coeff1);
	::GlobalFree(coeff2);
	::GlobalFree(coeff3);
}

#else

using namespace boost::multiprecision;
using boost::math::constants::pi;

void createHannCoeff(int tapNum, long long* dest, double* dest2)
{
	int coeffNum = (tapNum + 1) / 2;
	cpp_dec_float_100* coeff1 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);
	cpp_dec_float_100* coeff2 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);
	cpp_dec_float_100* coeff3 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);

	cpp_dec_float_100 piq = pi<cpp_dec_float_100>();

	coeff1[0] = cpp_dec_float_100(2) * 22050 / 352800;
	for (int i = 1; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = cpp_dec_float_100(i) * 2 * piq * 22050 / 352800;
		coeff1[i] = boost::multiprecision::sin(x) / (piq * i);
	}

	for (int i = 0; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = cpp_dec_float_100(2) * piq * i / (tapNum - 1);
		coeff2[i] = cpp_dec_float_100("0.5") + cpp_dec_float_100("0.5") * boost::multiprecision::cos(x);
	}
	coeff2[coeffNum - 1] = 0;

	long long scale = 1LL << (COEFF_SCALE + 3);

	for (int i = 0; i < coeffNum; ++i)
	{
		coeff3[i] = coeff1[i] * coeff2[i] * scale;
		//coeff3[i] = boost::multiprecision::round(coeff1[i] * coeff2[i] * scale);
	}

	dest[coeffNum - 1] = (long long)boost::multiprecision::round(coeff3[0]);
	dest2[coeffNum - 1] = (double)(coeff3[0] - boost::multiprecision::round(coeff3[0]));
	for (int i = 1; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = boost::multiprecision::round(coeff3[i]);
		dest[coeffNum - 1 + i] = (long long)x;
		dest[coeffNum - 1 - i] = (long long)x;
		dest2[coeffNum - 1 + i] = (double)(coeff3[i] - x);
		dest2[coeffNum - 1 - i] = (double)(coeff3[i] - x);
	}
	::GlobalFree(coeff1);
	::GlobalFree(coeff2);
	::GlobalFree(coeff3);
}

#endif

static void writeRaw32bitPCM(long long left, long long right, int* buffer)
{
	int shift = SCALE_SHIFT;

	int add = 1 << (shift - 1);
	left += add;
	right += add;

	if (left >= 4611686018427387904) left = 4611686018427387904 - 1; // over 63bit : limitted to under [1 << 62]   62bit + 1bit
	if (right >= 4611686018427387904) right = 4611686018427387904 - 1;

	if (left < -4611686018427387904) left = -4611686018427387904;
	if (right < -4611686018427387904) right = -4611686018427387904;

	left = left >> shift;
	right = right >> shift;

	buffer[0] = (int)left;
	buffer[1] = (int)right;
}

int  oversample(short* src, unsigned int length, long long* coeff, double* coeff2, int tapNum, int* dest, unsigned int option)
{
	int half_size = (tapNum - 1) / 2;
	if (option == 0) option = 0xffff;

	for (unsigned int i = 0; i < length; ++i)
	{
		short *srcLeft = src;
		short *srcRight = src + 1;
		long long tmpLeft, tmpRight;
		double tmpLeft2, tmpRight2;

		if (option & 0x0001)
		{
			// 1st
			tmpLeft = *srcLeft * coeff[half_size];
			tmpRight = *srcRight * coeff[half_size];
			writeRaw32bitPCM(tmpLeft, tmpRight, dest);
		}

		if (option & 0x0002)
		{
			// 2nd
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			// src[1] * coeff[ 7]  +  src[ 2] * coeff[15]  +  src[ 3] * coeff[ 23]  + ...
			// src[0] * coeff[-1]  +  src[-1] * coeff[-9]  +  src[-2] * coeff[-17]  + ...
			for (int j = 1; (j * 8 - 1) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 1];
				tmpRight += (long long)*(srcRight + j * 2) *coeff[half_size + j * 8 - 1];

				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 1];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 1];
				#endif
			}
			for (int j = 0; (j * 8 + 1) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 1];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 1];

				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 1];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 1];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 2);
		}

		if (option & 0x0004)
		{
			// 3rd
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			// src[1] * coeff[ 6]  +  src[ 2] * coeff[ 14]  +  src[ 3] * coeff[ 22]  + ...
			// src[0] * coeff[-2]  +  src[-1] * coeff[-10]  +  src[-2] * coeff[-18]  + ...
			for (int j = 1; (j * 8 - 2) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 2];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 2];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 2];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 2];
				#endif
			}
			for (int j = 0; (j * 8 + 2) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 2];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 2];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 2];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 2];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 4);
		}

		if (option & 0x0008)
		{
			// 4th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 3) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 3];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 3];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 3];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 3];
				#endif
			}
			for (int j = 0; (j * 8 + 3) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 3];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 3];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 3];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 3];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 6);
		}

		if (option & 0x0010)
		{
			//5th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 4) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 4];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 4];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 4];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 4];
				#endif
			}
			for (int j = 0; (j * 8 + 4) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 4];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 4];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 4];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 4];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 8);
		}

		if (option & 0x0020)
		{
			//6th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 5) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 5];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 5];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 5];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 5];
				#endif
			}
			for (int j = 0; (j * 8 + 5) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 5];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 5];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 5];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 5];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 10);
		}

		if (option & 0x0040)
		{
			//7th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 6) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 6];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 6];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 6];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 6];
				#endif
			}
			for (int j = 0; (j * 8 + 6) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 6];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 6];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 6];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 6];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 12);
		}

		if (option & 0x0080)
		{
			//8th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 7) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 7];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 7];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 7];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 7];
				#endif
			}
			for (int j = 0; (j * 8 + 7) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 7];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 7];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 7];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 7];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 14);
		}

		src += 2;
		dest += 8 * 2;
	}

	return 0;
}

struct oversample_info
{
	short* src;
	unsigned int length;
	long long* coeff;
	double* coeff2;
	int tapNum;
	int* dest;
	unsigned int option;
};

DWORD WINAPI ThreadFunc(LPVOID arg)
{
	struct oversample_info* info = (struct oversample_info*)arg;
	oversample(info->src, info->length, info->coeff, info->coeff2, info->tapNum, info->dest, info->option);
	return 0;
}

unsigned int searchFmtDataChunk(wchar_t* fileName, WAVEFORMATEX* wf, DWORD* offset, DWORD* size)
{
	HANDLE fileHandle;
	fileHandle = CreateFileW(fileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
	if (fileHandle == INVALID_HANDLE_VALUE)
	{
		return 0;
	}

	DWORD header[2];
	DWORD readSize;
	WORD  wav[8];
	DWORD riffSize, pos = 0;
	DWORD dataOffset, dataSize;
	::ReadFile(fileHandle, header, 8, &readSize, NULL);
	bool fmtFound = false, dataFound = false;

	if (readSize != 8)
	{
		CloseHandle(fileHandle);
		return 0;
	}

	if (header[0] != 0x46464952)
	{
		// not "RIFF"
		CloseHandle(fileHandle);
		return 0;
	}
	riffSize = header[1];

	::ReadFile(fileHandle, header, 4, &readSize, NULL);
	if (readSize != 4)
	{
		CloseHandle(fileHandle);
		return 0;
	}
	if (header[0] != 0x45564157)
	{
		// not "WAVE"
		CloseHandle(fileHandle);
		return 0;
	}
	pos += 4;

	while (pos < riffSize)
	{
		::ReadFile(fileHandle, header, 8, &readSize, NULL);
		if (readSize != 8)
		{
			break;
		}
		pos += 8;

		if (header[0] == 0x20746d66)
		{
			// "fmt "
			if (header[1] >= 16)
			{
				::ReadFile(fileHandle, wav, 16, &readSize, NULL);
				if (readSize != 16)
				{
					break;
				}
				fmtFound = true;
				if (header[1] > 16)
				{
					::SetFilePointer(fileHandle, header[1] - 16, 0, FILE_CURRENT);
				}
				pos += header[1];
			}
			else
			{
				::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
				pos += header[1];
			}
		}
		else if (header[0] == 0x61746164)
		{
			// "data"
			dataFound = true;
			dataOffset = ::SetFilePointer(fileHandle, 0, 0, FILE_CURRENT);
			dataSize = header[1];
			::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
			pos += header[1];
		}
		else
		{
			::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
			pos += header[1];
		}
		if (GetLastError() != NO_ERROR)
		{
			break;
		}
	}
	CloseHandle(fileHandle);

	if (dataFound && fmtFound)
	{
		*offset = dataOffset;
		*size = dataSize;
		wf->wFormatTag = wav[0]; //  1:LPCM   3:IEEE float
		wf->nChannels = wav[1]; //  1:Mono  2:Stereo
		wf->nSamplesPerSec = *(DWORD*)(wav + 2);  // 44100, 48000, 176400, 19200, 352800, 384000...
		wf->nAvgBytesPerSec = *(DWORD*)(wav + 4);
		wf->nBlockAlign = wav[6]; // 4@16bit/2ch,  6@24bit/2ch,   8@32bit/2ch
		wf->wBitsPerSample = wav[7]; // 16bit, 24bit, 32bit
		wf->cbSize = 0;
		return 1;
	}
	return 0;
}

DWORD readWavFile(wchar_t* fileName, void* readMem, DWORD readPos, DWORD readLength)
{
	HANDLE fileHandle;
	DWORD wavDataOffset, wavDataSize, readSize = 0;
	WAVEFORMATEX wf;

	if (!searchFmtDataChunk(fileName, &wf, &wavDataOffset, &wavDataSize))
	{
		return 0;
	}

	fileHandle = CreateFileW(fileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
	if (fileHandle == INVALID_HANDLE_VALUE)
	{
		return 0;
	}

	if (::SetFilePointer(fileHandle, wavDataOffset + readPos, 0, FILE_BEGIN) == INVALID_SET_FILE_POINTER)
	{
		if (GetLastError() != NO_ERROR)
		{
			// fail
			return 0;
		}
	}
	::ReadFile(fileHandle, readMem, readLength, &readSize, NULL);
	::CloseHandle(fileHandle);

	return readSize;
}

static int writePCM352_32_header(HANDLE fileHandle, unsigned long dataSize)
{
	WAVEFORMATEX wf;
	wf.wFormatTag = 0x01;
	wf.nChannels = 2;
	wf.nSamplesPerSec = 352800;
	wf.nAvgBytesPerSec = 352800 * 8; // 352800 * 4byte(32bit) * 2ch
	wf.nBlockAlign = 8; // 8bytes (32bit, 2ch) per sample
	wf.wBitsPerSample = 32;
	wf.cbSize = 0; // ignored. not written.

	DWORD writtenSize = 0;
	WriteFile(fileHandle, "RIFF", 4, &writtenSize, NULL);
	DWORD size = (dataSize + 44) - 8;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);
	WriteFile(fileHandle, "WAVE", 4, &writtenSize, NULL);
	WriteFile(fileHandle, "fmt ", 4, &writtenSize, NULL);
	size = 16;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);
	WriteFile(fileHandle, &wf, size, &writtenSize, NULL);
	WriteFile(fileHandle, "data", 4, &writtenSize, NULL);
	size = (DWORD)dataSize;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);

	return 0;
}

int wmain(int argc, wchar_t *argv[], wchar_t *envp[])
{
	DWORD wavDataOffset, wavDataSize, writtenSize, length, readSize = 0;
	WAVEFORMATEX wf;
	wchar_t* fileName;
	wchar_t* destFileName;

	if (argc < 2) return 0;
	fileName = argv[1];
	destFileName = argv[2];

	ULONGLONG elapsedTime = GetTickCount64();

	if (!searchFmtDataChunk(fileName, &wf, &wavDataOffset, &wavDataSize))
	{
		return 0;
	}
	int part = wavDataSize / DATA_UNIT_SIZE;
	if ((wavDataSize %  DATA_UNIT_SIZE) != 0) part += 1;

	void* mem1 = ::GlobalAlloc(GPTR, DATA_UNIT_SIZE * 3);
	void* mem2 = (char*)mem1 + DATA_UNIT_SIZE;
	void* mem3 = (char*)mem2 + DATA_UNIT_SIZE;

	void* memOut = ::GlobalAlloc(GPTR, DATA_UNIT_SIZE * 8 * 2);

	HANDLE fileOut = CreateFileW(destFileName, GENERIC_WRITE, 0, NULL, CREATE_ALWAYS /*CREATE_NEW*/, FILE_ATTRIBUTE_NORMAL, NULL);
	writePCM352_32_header(fileOut, wavDataSize * 8 * 2);

	long long* firCoeff = (long long*)::GlobalAlloc(GPTR, sizeof(long long) * TAP_SIZE);
	double* firCoeff2 = (double*)::GlobalAlloc(GPTR, sizeof(double) * TAP_SIZE);
	createHannCoeff(TAP_SIZE, firCoeff, firCoeff2);

	for (int i = 0; i <= part; ++i)
	{
		::SetThreadExecutionState(ES_SYSTEM_REQUIRED);

		length = readSize;
		::CopyMemory(mem1, mem2, DATA_UNIT_SIZE);
		::CopyMemory(mem2, mem3, DATA_UNIT_SIZE);
		::SecureZeroMemory(mem3, DATA_UNIT_SIZE);
		if (i != part) readSize = readWavFile(fileName, mem3, DATA_UNIT_SIZE * i, DATA_UNIT_SIZE);
		if (i == 0) continue;

		struct oversample_info info[8];
		info[0].src = (short* )mem2;
		info[0].length = length / 4;
		info[0].coeff = firCoeff;
		info[0].coeff2 = firCoeff2;
		info[0].tapNum = TAP_SIZE;
		info[0].dest = (int* )memOut;
		info[0].option = 0;

		// Single thread
		ThreadFunc((LPVOID)&info[0]);

		// Multi thread (use code below instead of above)
		/*
		HANDLE thread[8];
		DWORD threadId[8];
		for (int j = 0; j < 8; ++j)
		{
			info[j] = info[0];
			info[j].option = 1 << j;
			thread[j] = CreateThread(NULL, 0, ThreadFunc, (LPVOID)&info[j], 0, &threadId[j]);
		}
		::WaitForMultipleObjects(8, thread, TRUE, INFINITE);
		*/

		::WriteFile(fileOut, memOut, length * 8 * 2, &writtenSize, NULL);
		std::cout << "WavOverSampling: Progress  " << (i * 100) / part << " %\r";
	}
	elapsedTime = GetTickCount64() - elapsedTime;
	std::cout << "\nWavOverSampling: Completed.   " << (elapsedTime/1000) << "." << (elapsedTime % 1000) <<  " sec  \n";

	::FlushFileBuffers(fileOut);
	::CloseHandle(fileOut);

	::GlobalFree(mem1);
	::GlobalFree(memOut);
	::GlobalFree(firCoeff);
	::GlobalFree(firCoeff2);
	return 0;
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * 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.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Alexandra Zerck $
// $Authors: Eva Lange $
// --------------------------------------------------------------------------

#include <cmath>
#include <boost/math/special_functions/acosh.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <OpenMS/TRANSFORMATIONS/RAW2PEAK/OptimizePick.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>

namespace OpenMS
{
  namespace OptimizationFunctions
  {

    // Print the computed signal
    void printSignal(const gsl_vector * x, void * param, float resolution)
    {

      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(param)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(param)->peaks;
      std::cout << "Printing Signal" << std::endl;
      if (resolution == 1.)
      {
        // iterate over all points of the signal
        for (size_t current_point = 0; current_point < positions.size(); current_point++)
        {
          double computed_signal     = 0.;
          double current_position    = positions[current_point];

          // iterate over all peaks
          for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
          {
            // Store the current parameters for this peak
            double p_height          = gsl_vector_get(x, 4 * current_peak);
            double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
            double p_width           = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                       : gsl_vector_get(x, 4 * current_peak + 2);

            // is it a Lorentz or a Sech - Peak?
            if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
            {
              computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
            }
            else // It's a Sech - Peak
            {
              computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
            }
          }
          std::cerr << positions[current_point] << " " << computed_signal << std::endl;
        }
      }
      else
      {
        // Compute step width
        float sw = (positions[1] - positions[0]) / resolution;
        for (int i = 0; i < positions.size() * resolution; i++)
        {
          double computed_signal     = 0.;
          double current_position    = positions[0] + i * sw;

          // iterate over all peaks
          for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
          {
            // Store the current parameters for this peak
            double p_height          = gsl_vector_get(x, 4 * current_peak);
            double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
            double p_width           = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                       : gsl_vector_get(x, 4 * current_peak + 2);

            // is it a Lorentz or a Sech - Peak?
            if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
            {
              computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
            }
            else // It's a Sech - Peak
            {
              computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
            }
          }

          std::cerr.precision(writtenDigits<DoubleReal>(0.0));

          std::cerr << positions[0] + i * sw << " " << computed_signal << std::endl;
        }
      }
    }

    // Evaluation of the target function for nonlinear optimization.
    int residual(const gsl_vector * x, void * params, gsl_vector * f)
    {
      // According to the gsl conventions, x contains the parameters to be optimized.
      // In our case, this means that we store for each peak four consecutive values:
      //  - its height
      //  - its left width
      //  - its right width
      //  - its position
      //
      // Params might contain any additional parameters. We handle these using class members
      // instead.
      // The vector f is supposed to contain the result when we return from this function.
      // Note: GSL wants the values for each data point i as one component of the results vector
      std::vector<DoubleReal> & signal = static_cast<OptimizePick::Data *>(params)->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactors & penalties = static_cast<OptimizePick::Data *>(params)->penalties;
      // iterate over all points of the signal
      for (size_t current_point = 0; current_point < positions.size(); current_point++)
      {
        double computed_signal     = 0.;
        double current_position    = positions[current_point];
        double experimental_signal = signal[current_point];

        // iterate over all peaks
        for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          // Store the current parameters for this peak
          double p_height        = gsl_vector_get(x, 4 * current_peak);
          double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
          double p_width         = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                   : gsl_vector_get(x, 4 * current_peak + 2);

          // is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
          }
          else // It's a Sech - Peak
          {
            computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
          }
        }
        gsl_vector_set(f, current_point, computed_signal - experimental_signal);
      }

      double penalty = 0.;
//      struct PenaltyFactors* penalties = (struct PenaltyFactors *)params;
      double penalty_pos    = penalties.pos;
      double penalty_lwidth = penalties.lWidth;
      double penalty_rwidth = penalties.rWidth;

      // iterate over all peaks again to compute the penalties
      for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        double old_position = peaks[current_peak].mz_position;
        double old_width_l  = peaks[current_peak].left_width;
        double old_width_r  = peaks[current_peak].right_width;
        double p_position   = gsl_vector_get(x, 4 * current_peak + 3);
        double p_width_l        = gsl_vector_get(x, 4 * current_peak + 1);
        double p_width_r    = gsl_vector_get(x, 4 * current_peak + 2);

        //penalty += pow(p_position - old_position, 2) + pow(p_width_l - old_width_l, 2) + pow(p_width_r - old_width_r, 2);
        penalty +=      penalty_pos    * pow(p_position - old_position, 2)
                   + penalty_lwidth * pow(p_width_l - old_width_l, 2)
                   + penalty_rwidth * pow(p_width_r - old_width_r, 2);
      }

      gsl_vector_set(f, positions.size(), 100 * penalty);

      return GSL_SUCCESS;
    }

    /** Compute the Jacobian of the residual, where each row of the matrix corresponds to a
     *  point in the data.
     */
    int jacobian(const gsl_vector * x, void * params, gsl_matrix * J)
    {
      // For the conventions on x and params c.f. the commentary in residual()
      //
      // The matrix J is supposed to contain the result when we return from this function.
      // Note: GSL expects the Jacobian as follows:
      // - each row corresponds to one data point
      // - each column corresponds to one parameter
      // std::vector<DoubleReal>& signal = static_cast<OptimizePick::Data*> (params) ->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactors & penalties = static_cast<OptimizePick::Data *>(params)->penalties;
      // iterate over all points of the signal
      for (size_t current_point = 0; current_point < positions.size(); current_point++)
      {
        double current_position    = positions[current_point];

        // iterate over all peaks
        for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          // Store the current parameters for this peak
          double p_height        = gsl_vector_get(x, 4 * current_peak);
          double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
          double p_width         = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                   : gsl_vector_get(x, 4 * current_peak + 2);

          // is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            double diff      = current_position - p_position;
            double denom_inv = 1. / (1. + pow(p_width * diff, 2));

            double ddl_left  = (current_position <= p_position)
                               ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                 0;

            double ddl_right = (current_position  > p_position)
                               ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                 0;

            double ddx0          = -2 * p_height * pow(p_width, 2) * diff * pow(denom_inv, 2);

            gsl_matrix_set(J, current_point, 4 * current_peak, denom_inv);
            gsl_matrix_set(J, current_point, 4 * current_peak + 1, ddl_left);
            gsl_matrix_set(J, current_point, 4 * current_peak + 2, ddl_right);
            gsl_matrix_set(J, current_point, 4 * current_peak + 3, ddx0);
          }
          else // It's a Sech - Peak
          {
            double diff      = current_position - p_position;
            double denom_inv = 1. / cosh(p_width * diff);

            // The remaining computations are not stable if denom_inv == 0. In that case, we are far away from the peak
            // and can assume that all derivatives vanish
            double sinh_term = (fabs(denom_inv) < 1e-6) ? 0.0 : sinh(p_width * diff);
            double ddl_left  = (current_position <= p_position)
                               ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                 0;
            double ddl_right = (current_position  > p_position)
                               ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                 0;
            double ddx0      = 2 * p_height * p_width * sinh_term * pow(denom_inv, 3);

            gsl_matrix_set(J, current_point, 4 * current_peak, pow(denom_inv, 2));
            gsl_matrix_set(J, current_point, 4 * current_peak + 1, ddl_left);
            gsl_matrix_set(J, current_point, 4 * current_peak + 2, ddl_right);
            gsl_matrix_set(J, current_point, 4 * current_peak + 3, ddx0);
          }
        }
      }

      // Now iterate over all peaks again to compute the penalties.
//   struct PenaltyFactors* penalties = (struct PenaltyFactors *)params;
      for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        double p_width_left = gsl_vector_get(x, 4 * current_peak + 1);
        double p_width_right = gsl_vector_get(x, 4 * current_peak + 2);
        double p_position   = gsl_vector_get(x, 4 * current_peak + 3);

        double old_width_left  = peaks[current_peak].left_width;
        double old_width_right = peaks[current_peak].right_width;
        double old_position    = peaks[current_peak].mz_position;

        double penalty_l = 2. * penalties.lWidth * (p_width_left - old_width_left);
        double penalty_r = 2. * penalties.rWidth * (p_width_right - old_width_right);
        double penalty_p = 0;
        if (fabs(p_position - old_position) < 0.2)
        {
          penalty_p = 2. * penalties.pos * (p_position - old_position);
        }

        gsl_matrix_set(J, positions.size(), 4 * current_peak, 0.);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 1, 100 * penalty_l);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 2, 100 * penalty_r);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 3, 100 * penalty_p);
      }

      return GSL_SUCCESS;
    }

    // Driver function for the evaluation of function and jacobian.
    int evaluate(const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix * J)
    {
      residual(x, params, f);
      jacobian(x, params, J);

      return GSL_SUCCESS;
    }

  }

  OptimizePick::OptimizePick(const struct OptimizationFunctions::PenaltyFactors & penalties,
                             const int max_iteration,
                             const double eps_abs,
                             const double eps_rel)
  {

    penalties_ = penalties;

    max_iteration_ = max_iteration;
    eps_abs_ = eps_abs;
    eps_rel_ = eps_rel;

#ifdef DEBUG_PEAK_PICKING
    std::cout << "max iteration " << max_iteration_
              << "\n eps abs " << eps_abs
              << "\n eps rel " << eps_rel
              << "\n penalty factor pos " << penalties.pos
              << "\n penalty factor left width " << penalties.lWidth
              << "\n penalty factor right width " << penalties.rWidth
              << std::endl;
#endif

  }

  OptimizePick::~OptimizePick()
  {
  }

  void OptimizePick::optimize(std::vector<PeakShape> & peaks, Data & data)
  {
    if (peaks.empty())
      return;

    size_t global_peak_number = 0;
    data.peaks.assign(peaks.begin(), peaks.end());

    gsl_vector * start_value = gsl_vector_alloc(4 * data.peaks.size());
    // We have to initialize the parameters for the optimization
    for (size_t i = 0; i < data.peaks.size(); i++)
    {
      PeakShape current_peak = data.peaks[i];
      double h  = current_peak.height;
      double wl = current_peak.left_width;
      double wr = current_peak.right_width;
      double p  = current_peak.mz_position;
      if (boost::math::isnan(wl))
      {
        data.peaks[i].left_width = 1;
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        data.peaks[i].right_width = 1;
        wr = 1.;
      }

      gsl_vector_set(start_value, 4 * i, h);
      gsl_vector_set(start_value, 4 * i + 1, wl);
      gsl_vector_set(start_value, 4 * i + 2, wr);
      gsl_vector_set(start_value, 4 * i + 3, p);
    }

    // The gsl algorithms require us to provide function pointers for the evaluation of
    // the target function.
    gsl_multifit_function_fdf fit_function;
    fit_function.f      = OptimizationFunctions::residual;
    fit_function.df       = OptimizationFunctions::jacobian;
    fit_function.fdf        = OptimizationFunctions::evaluate;
    fit_function.n          = std::max(data.positions.size() + 1, 4 * data.peaks.size());
    fit_function.p          = 4 * data.peaks.size();
//    fit_function.params = &penalties_;
    data.penalties = penalties_;
    fit_function.params = &data;

    const gsl_multifit_fdfsolver_type * type = gsl_multifit_fdfsolver_lmsder;

    gsl_multifit_fdfsolver * fit = gsl_multifit_fdfsolver_alloc(type, std::max(data.positions.size() + 1, 4 * data.peaks.size()), 4 * data.peaks.size());

    gsl_multifit_fdfsolver_set(fit, &fit_function, start_value);

    // initial norm
    // std::cout << "Before optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;

    // Iteration
    unsigned int iteration = 0;
    int status;

    do
    {
      iteration++;
      status = gsl_multifit_fdfsolver_iterate(fit);
#ifdef DEBUG_PEAK_PICKING
      std::cout << "Iteration " << iteration << "; Status " << gsl_strerror(status) << "; " << std::endl;
      std::cout << "||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
      std::cout << "Number of parms: " << data.peaks.size() * 4 << std::endl;
      std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
#endif
      if (boost::math::isnan(gsl_blas_dnrm2(fit->dx)))
        break;

      // We use the gsl function gsl_multifit_test_delta to decide if we can finish the iteration.
      // We only finish if all new parameters deviates only by a small amount from the parameters of the last iteration
      status = gsl_multifit_test_delta(fit->dx, fit->x, eps_abs_, eps_rel_);
      if (status != GSL_CONTINUE)
        break;

    }
    while (status == GSL_CONTINUE && iteration < max_iteration_);

#ifdef DEBUG_PEAK_PICKING
    std::cout << "Finished!" << std::endl;
    std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
    double chi = gsl_blas_dnrm2(fit->f);
    std::cout << "chisq/dof = " << pow(chi, 2.0) / (data.positions.size() - 4 * data.peaks.size());
#endif

    // OptimizationFunctions::printSignal(fit->x, 5.,param);

    // iterate over all peaks and store the optimized values in peaks
    for (size_t current_peak = 0; current_peak < data.peaks.size(); current_peak++)
    {
      // Store the current parameters for this peak
      peaks[global_peak_number + current_peak].height          = gsl_vector_get(fit->x, 4 * current_peak);
      peaks[global_peak_number + current_peak].mz_position = gsl_vector_get(fit->x, 4 * current_peak + 3);
      peaks[global_peak_number + current_peak].left_width  = gsl_vector_get(fit->x, 4 * current_peak + 1);
      peaks[global_peak_number + current_peak].right_width = gsl_vector_get(fit->x, 4 * current_peak + 2);

      // compute the area
      // is it a Lorentz or a Sech - Peak?
      if (peaks[global_peak_number + current_peak].type == PeakShape::LORENTZ_PEAK)
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * sqrt(p.height / 1 - 1);
        double x_rigth_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
        double area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
        double area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_rigth_endpoint));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Lorentz " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
#endif
      }
      else  //It's a Sech - Peak
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
        double x_rigth_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
        double area_left = p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint)) / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
        double area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_rigth_endpoint)) / cosh(p.right_width * (p.mz_position - x_rigth_endpoint)));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Sech " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
        std::cout << p.mz_position << " " << x_left_endpoint << " " << x_rigth_endpoint << std::endl;
#endif
      }
    }
    global_peak_number += data.peaks.size();

    gsl_multifit_fdfsolver_free(fit);
    gsl_vector_free(start_value);
  }

  // double OptimizePick::correlate_(const PeakShape& peak,
//                                  double left_endpoint,
//                                                                  double right_endpoint,Data& data)
//   {
//     double SSxx = 0., SSyy = 0., SSxy = 0.;

//     // compute the averages
//     double data_average=0., fit_average=0.;
//     double data_sqr=0., fit_sqr=0.;
//     double cross=0.;

//     int number_of_points = 0;

//     int first=0;
//     int last=data.positions.size()-1;

//     // search for the left endpoint position
//     while (data.positions[first] < left_endpoint)
//     {
//       ++first;
//     }

//     // search for the right endpoint position
//     while (data.positions[last] > right_endpoint)
//     {
//       --last;
//     }

//     // for separate overlapping peak correlate until the max position...
//     for (int i=first; i <= last; i++)
//     {
//       double data_val = data.signal[i];
//       double peak_val = peak(data.positions[i]);

//       data_average += data_val;
//       fit_average  += peak_val;

//       data_sqr += data_val * data_val;
//       fit_sqr  += peak_val * peak_val;

//       cross += data_val * peak_val;

//       number_of_points++;
//     }

//     if (number_of_points == 0)
//       return 0.;

//     data_average /= number_of_points;
//     fit_average  /= number_of_points;

//     SSxx = data_sqr - number_of_points * (data_average * data_average);
//     SSyy = fit_sqr - number_of_points * (fit_average * fit_average);
//     SSxy = cross - number_of_points * (data_average * fit_average);

//     return (SSxy * SSxy) / (SSxx * SSyy);
//   }

}


/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * 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 <x/vio/msckf_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

MsckfUpdate::MsckfUpdate(const x::TrackList& trks,
                         const x::AttitudeList& quats,
                         const x::TranslationList& pos,
                         const Triangulation& triangulator,
                         const MatrixXd& cov_s,
                         const int n_poses_max,
                         const double sigma_img)
{
  // Number of features
  const size_t n_trks = trks.size();

  // Number of feature observations
  size_t n_obs = 0;
  for(size_t i=0; i < n_trks; i++)
    n_obs += trks[i].size();

  // Initialize Kalman update matrices
  const size_t rows = 2 * n_obs - n_trks * 3;
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(rows, cols);
  cov_m_diag_ = VectorXd::Ones(rows);
  res_ = MatrixXd::Zero(rows, 1);

  // For each track, compute residual, Jacobian and covariance block
  const double var_img = sigma_img * sigma_img;
  for (size_t i = 0, row_h = 0; i < n_trks; ++i) {
    processOneTrack(trks[i],
                    quats,
                    pos,
                    triangulator,
                    cov_s,
                    n_poses_max,
                    var_img,
                    i,
                    row_h);
  }
}

void MsckfUpdate::processOneTrack(const x::Track& track,
                                  const x::AttitudeList& C_q_G,
                                  const x::TranslationList& G_p_C,
                                  const Triangulation& triangulator,
                                  const MatrixXd& P,
                                  const int n_poses_max,
                                  const double var_img,
                                  const size_t& j,
                                  size_t& row_h)
{
  // Initialization
  const size_t track_size = track.size();
  unsigned int rows_track_j = track_size * 2;
  const size_t cols = P.cols();
  MatrixXd jac_j(MatrixXd::Zero(rows_track_j, cols));
  MatrixXd Hf_j(MatrixXd::Zero(jac_j.rows(), kJacCols));
  MatrixXd res_j(MatrixXd::Zero(rows_track_j, 1));

  // Triangulate feature j
  // Inverse-depth parameters in last observation frame
  Vector3d feature; // inverse-depth parameters in last observation frame
  triangulator.triangulateGN(track, feature);
  const double alpha = feature(0);
  const double beta  = feature(1);
  const double rho   = feature(2);

  // Coordinate of feature in global frame
  x::Quaternion Cn_q_G;
  Cn_q_G.x() = C_q_G.back().ax;
  Cn_q_G.y() = C_q_G.back().ay;
  Cn_q_G.z() = C_q_G.back().az;
  Cn_q_G.w() = C_q_G.back().aw;

  Vector3d G_p_Cn(G_p_C.back().tx, G_p_C.back().ty, G_p_C.back().tz);

  Vector3d G_p_fj = 1 / (rho)*Cn_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Cn;

  x::Quatern attitude_to_quaternion;

  // LOOP OVER ALL FEATURE OBSERVATIONS
  for (size_t i = 0; i < track_size; ++i)
  {
    const unsigned int pos = C_q_G.size() - track_size + i;

    Quaterniond Ci_q_G_ = attitude_to_quaternion(C_q_G[pos]);
    Vector3d G_p_Ci_(G_p_C[pos].tx, G_p_C[pos].ty, G_p_C[pos].tz);

    // Feature position expressed in camera frame.
    Vector3d Ci_p_fj;
    Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

    // eq. 20(a)
    Vector2d z;
    z(0) = track[i].getX();
    z(1) = track[i].getY();

    Vector2d z_hat(z);
    assert(Ci_p_fj(2));
    z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2);
    z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2);

    // eq. 20(b)
    res_j(i * 2, 0) = z(0) - z_hat(0);
    res_j(i * 2 + 1, 0) = z(1) - z_hat(1);

    // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
    VisJacBlock J_i = VisJacBlock::Zero();
    // first row
    J_i(0, 0) = 1.0 / Ci_p_fj(2);
    J_i(0, 1) = 0.0;
    J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2);
    // second row
    J_i(1, 0) = 0.0;
    J_i(1, 1) = 1.0 / Ci_p_fj(2);
    J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2);

    unsigned int row = i * kVisJacRows;

    // Measurement Jacobians wrt attitude, position and feature
    // Position
    VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

    Vector3d dP = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

    // Attitude
    VisJacBlock J_attitude = J_i * x::Skew(dP(0), dP(1), dP(2)).matrix;

    // Feature
    Hf_j.block<kVisJacRows, kJacCols>(row, 0) = -J_position;

    // Update stacked Jacobian matrix associated to the current feature
    unsigned int col = pos * kJacCols;
    jac_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_position;

    col += n_poses_max * kJacCols;

    jac_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_attitude;
  }  // LOOP OVER ALL FEATURE OBSERVATIONS

  //========================================================================
  // Left nullspace projection
  //========================================================================
  // Nullspace computation
  MatrixXd q = Hf_j.householderQr().householderQ();
  MatrixXd A = x::MatrixBlock(q, 0, 3);

  // Projections
  MatrixXd res0_j = A.transpose() * res_j;
  MatrixXd jac0_j = A.transpose() * jac_j;

  // Noise measurement matrix
  const VectorXd r0_j_diag = var_img * VectorXd::Ones(rows_track_j - 3);
  const MatrixXd r0_j = r0_j_diag.asDiagonal();

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  MatrixXd S_inv = (jac0_j * P * jac0_j.transpose() + r0_j).inverse();
  MatrixXd gamma = res0_j.transpose() * S_inv * res0_j;
  boost::math::chi_squared_distribution<> my_chisqr(2 * track_size - 3); // 2*Mj-3 DOFs
  double chi = quantile(my_chisqr, 0.95); // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
#ifdef VERBOSE
    inliers_.push_back(G_p_fj);
#endif

    jac_.block(row_h,            // startRow
             0,                 // startCol
             rows_track_j - 3,  // numRows
             cols) = jac0_j;    // numCols

    // Residual vector (for this track)
    res_.block(row_h, 0, rows_track_j - 3, 1) = res0_j;

    // Measurement covariance matrix diagonal
    cov_m_diag_.segment(row_h, rows_track_j - 3) = r0_j_diag;

    row_h += rows_track_j - 3;
  }
  else  // outlier
  {
#ifdef VERBOSE
    outliers_.push_back(G_p_fj);
#endif
  }
}


#include "camera_model/camera_models/ScaramuzzaCamera.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camera_model/gpl/gpl.h"

Eigen::VectorXd
polyfit( Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order )
{
    assert( poly_order > 0 );
    assert( xVec.size( ) > poly_order );
    assert( xVec.size( ) == yVec.size( ) );

    Eigen::MatrixXd A( xVec.size( ), poly_order + 1 );
    Eigen::VectorXd B( xVec.size( ) );

    for ( int i = 0; i < xVec.size( ); ++i )
    {
        const double x = xVec( i );
        const double y = yVec( i );

        double x_pow_k = 1.0;

        for ( int k = 0; k <= poly_order; ++k )
        {
            A( i, k ) = x_pow_k;
            x_pow_k *= x;
        }

        B( i ) = y;
    }

    Eigen::JacobiSVD< Eigen::MatrixXd > svd( A, Eigen::ComputeThinU | Eigen::ComputeThinV );
    Eigen::VectorXd x = svd.solve( B );

    return x;
}

namespace camera_model
{

OCAMCamera::Parameters::Parameters( )
: Camera::Parameters( SCARAMUZZA )
, m_C( 0.0 )
, m_D( 0.0 )
, m_E( 0.0 )
, m_center_x( 0.0 )
, m_center_y( 0.0 )
{
    memset( m_poly, 0, sizeof( double ) * SCARAMUZZA_POLY_SIZE );
    memset( m_inv_poly, 0, sizeof( double ) * SCARAMUZZA_INV_POLY_SIZE );
}

bool
OCAMCamera::Parameters::readFromYamlFile( const std::string& filename )
{
    cv::FileStorage fs( filename, cv::FileStorage::READ );

    if ( !fs.isOpened( ) )
    {
        return false;
    }

    if ( !fs["model_type"].isNone( ) )
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;

        if ( !boost::iequals( sModelType, "scaramuzza" ) )
        {
            return false;
        }
    }

    m_modelType = SCARAMUZZA;
    fs["camera_name"] >> m_cameraName;
    m_imageWidth  = static_cast< int >( fs["image_width"] );
    m_imageHeight = static_cast< int >( fs["image_height"] );

    cv::FileNode n = fs["poly_parameters"];
    for ( int i   = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        m_poly[i] = static_cast< double >( n[std::string( "p" ) + boost::lexical_cast< std::string >( i )] );

    n = fs["inv_poly_parameters"];
    for ( int i       = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        m_inv_poly[i] = static_cast< double >( n[std::string( "p" ) + boost::lexical_cast< std::string >( i )] );

    n   = fs["affine_parameters"];
    m_C = static_cast< double >( n["ac"] );
    m_D = static_cast< double >( n["ad"] );
    m_E = static_cast< double >( n["ae"] );

    m_center_x = static_cast< double >( n["cx"] );
    m_center_y = static_cast< double >( n["cy"] );

    return true;
}

void
OCAMCamera::Parameters::writeToYamlFile( const std::string& filename ) const
{
    cv::FileStorage fs( filename, cv::FileStorage::WRITE );

    fs << "model_type"
       << "scaramuzza";
    fs << "camera_name" << m_cameraName;
    fs << "image_width" << m_imageWidth;
    fs << "image_height" << m_imageHeight;

    fs << "poly_parameters";
    fs << "{";
    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        fs << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << m_poly[i];
    fs << "}";

    fs << "inv_poly_parameters";
    fs << "{";
    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        fs << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << m_inv_poly[i];
    fs << "}";

    fs << "affine_parameters";
    fs << "{"
       << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" << m_center_y << "}";

    fs.release( );
}

OCAMCamera::Parameters&
OCAMCamera::Parameters::operator=( const OCAMCamera::Parameters& other )
{
    if ( this != &other )
    {
        m_modelType   = other.m_modelType;
        m_cameraName  = other.m_cameraName;
        m_imageWidth  = other.m_imageWidth;
        m_imageHeight = other.m_imageHeight;
        m_C           = other.m_C;
        m_D           = other.m_D;
        m_E           = other.m_E;
        m_center_x    = other.m_center_x;
        m_center_y    = other.m_center_y;

        memcpy( m_poly, other.m_poly, sizeof( double ) * SCARAMUZZA_POLY_SIZE );
        memcpy( m_inv_poly, other.m_inv_poly, sizeof( double ) * SCARAMUZZA_INV_POLY_SIZE );
    }

    return *this;
}

std::ostream&
operator<<( std::ostream& out, const OCAMCamera::Parameters& params )
{
    out << "Camera Parameters:" << std::endl;
    out << "    model_type "
        << "scaramuzza" << std::endl;
    out << "   camera_name " << params.m_cameraName << std::endl;
    out << "   image_width " << params.m_imageWidth << std::endl;
    out << "  image_height " << params.m_imageHeight << std::endl;

    out << std::fixed << std::setprecision( 10 );

    out << "Poly Parameters" << std::endl;
    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        out << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << ": " << params.m_poly[i] << std::endl;

    out << "Inverse Poly Parameters" << std::endl;
    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        out << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << ": " << params.m_inv_poly[i] << std::endl;

    out << "Affine Parameters" << std::endl;
    out << "            ac " << params.m_C << std::endl
        << "            ad " << params.m_D << std::endl
        << "            ae " << params.m_E << std::endl;
    out << "            cx " << params.m_center_x << std::endl
        << "            cy " << params.m_center_y << std::endl;

    return out;
}

OCAMCamera::OCAMCamera( )
: m_inv_scale( 0.0 )
{
}

OCAMCamera::OCAMCamera( const OCAMCamera::Parameters& params )
: mParameters( params )
{
    m_inv_scale = 1.0 / ( params.C( ) - params.D( ) * params.E( ) );
}

Camera::ModelType
OCAMCamera::modelType( void ) const
{
    return mParameters.modelType( );
}

const std::string&
OCAMCamera::cameraName( void ) const
{
    return mParameters.cameraName( );
}

int
OCAMCamera::imageWidth( void ) const
{
    return mParameters.imageWidth( );
}

int
OCAMCamera::imageHeight( void ) const
{
    return mParameters.imageHeight( );
}

void
OCAMCamera::estimateIntrinsics( const cv::Size& boardSize,
                                const std::vector< std::vector< cv::Point3f > >& objectPoints,
                                const std::vector< std::vector< cv::Point2f > >& imagePoints )
{
    // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" <<
    // std::endl;
    // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");

    // Reference: Page 30 of
    // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion
    // Estimation, ETH Zurich. Thesis no. 17635."
    // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
    // Matlab code: calibrate.m

    // First, estimate every image's extrinsics parameters
    std::vector< Eigen::Matrix3d > RList;
    std::vector< Eigen::Vector3d > TList;

    RList.reserve( imagePoints.size( ) );
    TList.reserve( imagePoints.size( ) );

    // i-th image
    for ( size_t image_index = 0; image_index < imagePoints.size( ); ++image_index )
    {
        const std::vector< cv::Point3f >& objPts = objectPoints.at( image_index );
        const std::vector< cv::Point2f >& imgPts = imagePoints.at( image_index );

        assert( objPts.size( ) == imgPts.size( ) );
        assert( objPts.size( ) == static_cast< unsigned int >( boardSize.width * boardSize.height ) );

        Eigen::MatrixXd M( objPts.size( ), 6 );

        for ( size_t corner_index = 0; corner_index < objPts.size( ); ++corner_index )
        {
            double X = objPts.at( corner_index ).x;
            double Y = objPts.at( corner_index ).y;
            assert( objPts.at( corner_index ).z == 0.0 );

            double u = imgPts.at( corner_index ).x;
            double v = imgPts.at( corner_index ).y;

            M( corner_index, 0 ) = -v * X;
            M( corner_index, 1 ) = -v * Y;
            M( corner_index, 2 ) = u * X;
            M( corner_index, 3 ) = u * Y;
            M( corner_index, 4 ) = -v;
            M( corner_index, 5 ) = u;
        }

        Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeFullU | Eigen::ComputeFullV );
        assert( svd.matrixV( ).cols( ) == 6 );
        Eigen::VectorXd h = -svd.matrixV( ).col( 5 );

        // scaled version of R and T
        const double sr11 = h( 0 );
        const double sr12 = h( 1 );
        const double sr21 = h( 2 );
        const double sr22 = h( 3 );
        const double st1  = h( 4 );
        const double st2  = h( 5 );

        const double AA = square( sr11 * sr12 + sr21 * sr22 );
        const double BB = square( sr11 ) + square( sr21 );
        const double CC = square( sr12 ) + square( sr22 );

        const double sr32_squared_1 = ( -( CC - BB ) + sqrt( square( CC - BB ) + 4.0 * AA ) ) / 2.0;
        const double sr32_squared_2 = ( -( CC - BB ) - sqrt( square( CC - BB ) + 4.0 * AA ) ) / 2.0;

        // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 +
        // (CC-BB)*sr32_squared_1 - AA);

        std::vector< double > sr32_squared_values;
        if ( sr32_squared_1 > 0 )
            sr32_squared_values.push_back( sr32_squared_1 );
        if ( sr32_squared_2 > 0 )
            sr32_squared_values.push_back( sr32_squared_2 );
        assert( !sr32_squared_values.empty( ) );

        std::vector< double > sr32_values;
        std::vector< double > sr31_values;
        for ( auto sr32_squared : sr32_squared_values )
        {
            for ( int sign = -1; sign <= 1; sign += 2 )
            {
                const double sr32 = static_cast< double >( sign ) * std::sqrt( sr32_squared );
                sr32_values.push_back( sr32 );
                if ( sr32_squared == 0.0 )
                {
                    // sr31 can be calculated through norm equality,
                    // but it has positive and negative posibilities
                    // positive one
                    sr31_values.push_back( std::sqrt( CC - BB ) );
                    // negative one
                    sr32_values.push_back( sr32 );
                    sr31_values.push_back( -std::sqrt( CC - BB ) );

                    break; // skip the same situation
                }
                else
                {
                    // sr31 can be calculated throught dot product == 0
                    sr31_values.push_back( -( sr11 * sr12 + sr21 * sr22 ) / sr32 );
                }
            }
        }

        // std::cout << "h= " << std::setprecision(12) << h.transpose() <<
        // std::endl;
        // std::cout << "length: " << sr32_values.size() << " & " <<
        // sr31_values.size() << std::endl;

        assert( !sr31_values.empty( ) );
        assert( sr31_values.size( ) == sr32_values.size( ) );

        std::vector< Eigen::Matrix3d > H_values;
        for ( size_t i = 0; i < sr31_values.size( ); ++i )
        {
            const double sr31   = sr31_values.at( i );
            const double sr32   = sr32_values.at( i );
            const double lambda = 1.0 / sqrt( sr11 * sr11 + sr21 * sr21 + sr31 * sr31 );
            Eigen::Matrix3d H;
            H.setZero( );
            H( 0, 0 ) = sr11;
            H( 0, 1 ) = sr12;
            H( 0, 2 ) = st1;
            H( 1, 0 ) = sr21;
            H( 1, 1 ) = sr22;
            H( 1, 2 ) = st2;
            H( 2, 0 ) = sr31;
            H( 2, 1 ) = sr32;
            H( 2, 2 ) = 0;

            H_values.push_back( lambda * H );
            H_values.push_back( -lambda * H );
        }

        for ( auto& H : H_values )
        {
            // std::cout << "H=\n" << H << std::endl;
            Eigen::Matrix3d R;
            R.col( 0 ) = H.col( 0 );
            R.col( 1 ) = H.col( 1 );
            R.col( 2 ) = H.col( 0 ).cross( H.col( 1 ) );
            // std::cout << "R33 = " << R(2,2) << std::endl;
        }

        std::vector< Eigen::Matrix3d > H_candidates;

        for ( auto& H : H_values )
        {
            Eigen::MatrixXd A_mat( 2 * imagePoints.at( image_index ).size( ), 4 );
            Eigen::VectorXd B_vec( 2 * imagePoints.at( image_index ).size( ) );
            A_mat.setZero( );
            B_vec.setZero( );

            size_t line_index = 0;

            // iterate images
            const double& r11 = H( 0, 0 );
            const double& r12 = H( 0, 1 );
            // const double& r13 = H(0,2);
            const double& r21 = H( 1, 0 );
            const double& r22 = H( 1, 1 );
            // const double& r23 = H(1,2);
            const double& r31 = H( 2, 0 );
            const double& r32 = H( 2, 1 );
            // const double& r33 = H(2,2);
            const double& t1 = H( 0 );
            const double& t2 = H( 1 );

            // iterate chessboard corners in the image
            for ( size_t j = 0; j < imagePoints.at( image_index ).size( ); ++j )
            {
                assert( line_index == 2 * j );

                const double& X = objectPoints.at( image_index ).at( j ).x;
                const double& Y = objectPoints.at( image_index ).at( j ).y;
                const double& u = imagePoints.at( image_index ).at( j ).x;
                const double& v = imagePoints.at( image_index ).at( j ).y;

                double A   = r21 * X + r22 * Y + t2;
                double B   = v * ( r31 * X + r32 * Y );
                double C   = r11 * X + r12 * Y + t1;
                double D   = u * ( r31 * X + r32 * Y );
                double rou = std::sqrt( u * u + v * v );

                A_mat( line_index + 0, 0 ) = A;
                A_mat( line_index + 1, 0 ) = C;
                A_mat( line_index + 0, 1 ) = A * rou;
                A_mat( line_index + 1, 1 ) = C * rou;
                A_mat( line_index + 0, 2 ) = A * rou * rou;
                A_mat( line_index + 1, 2 ) = C * rou * rou;

                A_mat( line_index + 0, 3 ) = -v;
                A_mat( line_index + 1, 3 ) = -u;
                B_vec( line_index + 0 ) = B;
                B_vec( line_index + 1 ) = D;

                line_index += 2;
            }

            assert( line_index == static_cast< unsigned int >( A_mat.rows( ) ) );

            // pseudo-inverse for polynomial parameters and all t3s
            {
                Eigen::JacobiSVD< Eigen::MatrixXd > svd( A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV );

                Eigen::VectorXd x = svd.solve( B_vec );

                // std::cout << "x(poly and t3) = " << x << std::endl;

                if ( x( 2 ) > 0 && x( 3 ) > 0 )
                {
                    H_candidates.push_back( H );
                }
            }
        }

        // printf("H_candidates.size()=%zu\n", H_candidates.size());
        assert( H_candidates.size( ) == 1 );

        Eigen::Matrix3d& H = H_candidates.front( );

        Eigen::Matrix3d R;
        R.col( 0 ) = H.col( 0 );
        R.col( 1 ) = H.col( 1 );
        R.col( 2 ) = H.col( 0 ).cross( H.col( 1 ) );

        Eigen::Vector3d T = H.col( 2 );
        RList.push_back( R );
        TList.push_back( T );

        // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT =
        // " << T.transpose() << std::endl;
    }

    // Second, estimate camera intrinsic parameters and all t3
    Eigen::MatrixXd A_mat( 2 * imagePoints.size( ) * imagePoints.at( 0 ).size( ),
                           SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size( ) );
    Eigen::VectorXd B_vec( 2 * imagePoints.size( ) * imagePoints.at( 0 ).size( ) );
    A_mat.setZero( );
    B_vec.setZero( );

    size_t line_index = 0;

    // iterate images
    for ( size_t i = 0; i < imagePoints.size( ); ++i )
    {
        const double& r11 = RList.at( i )( 0, 0 );
        const double& r12 = RList.at( i )( 0, 1 );
        // const double& r13 = RList.at(i)(0,2);
        const double& r21 = RList.at( i )( 1, 0 );
        const double& r22 = RList.at( i )( 1, 1 );
        // const double& r23 = RList.at(i)(1,2);
        const double& r31 = RList.at( i )( 2, 0 );
        const double& r32 = RList.at( i )( 2, 1 );
        // const double& r33 = RList.at(i)(2,2);
        const double& t1 = TList.at( i )( 0 );
        const double& t2 = TList.at( i )( 1 );

        // iterate chessboard corners in the image
        for ( size_t j = 0; j < imagePoints.at( i ).size( ); ++j )
        {
            assert( line_index == 2 * ( i * imagePoints.at( 0 ).size( ) + j ) );

            const double& X = objectPoints.at( i ).at( j ).x;
            const double& Y = objectPoints.at( i ).at( j ).y;
            const double& u = imagePoints.at( i ).at( j ).x;
            const double& v = imagePoints.at( i ).at( j ).y;

            double A   = r21 * X + r22 * Y + t2;
            double B   = v * ( r31 * X + r32 * Y );
            double C   = r11 * X + r12 * Y + t1;
            double D   = u * ( r31 * X + r32 * Y );
            double rou = std::sqrt( u * u + v * v );

            for ( int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k )
            {
                double pow_rou = 0.0;
                if ( k == 1 )
                {
                    pow_rou = 1.0;
                }
                else
                {
                    pow_rou = std::pow( rou, k );
                }

                A_mat( line_index + 0, k - 1 ) = A * pow_rou;
                A_mat( line_index + 1, k - 1 ) = C * pow_rou;
            }

            A_mat( line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i ) = -v;
            A_mat( line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i ) = -u;
            B_vec( line_index + 0 ) = B;
            B_vec( line_index + 1 ) = D;

            line_index += 2;
        }
    }

    assert( line_index == static_cast< unsigned int >( A_mat.rows( ) ) );

    Eigen::Matrix< double, SCARAMUZZA_POLY_SIZE, 1 > poly_coeff;
    // pseudo-inverse for polynomial parameters and all t3s
    {
        Eigen::JacobiSVD< Eigen::MatrixXd > svd( A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV );

        Eigen::VectorXd x = svd.solve( B_vec );

        poly_coeff[0] = x( 0 );
        poly_coeff[1] = 0.0;
        for ( int i = 1; i < poly_coeff.size( ) - 1; ++i )
        {
            poly_coeff[i + 1] = x( i );
        }
        assert( x.size( ) == static_cast< unsigned int >( SCARAMUZZA_POLY_SIZE - 1 + TList.size( ) ) );
    }

    Parameters params = getParameters( );

    // Affine matrix A is constructed as [C D; E 1]
    params.C( ) = 1.0;
    params.D( ) = 0.0;
    params.E( ) = 0.0;

    params.center_x( ) = params.imageWidth( ) / 2.0;
    params.center_y( ) = params.imageHeight( ) / 2.0;

    for ( size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i )
    {
        params.poly( i ) = poly_coeff[i];
    }

    // params.poly(0) = -216.9657476318;
    // params.poly(1) = 0.0;
    // params.poly(2) = 0.0017866911;
    // params.poly(3) = -0.0000019866;
    // params.poly(4) =  0.0000000077;

    // inv_poly
    {
        std::vector< double > rou_vec;
        std::vector< double > z_vec;
        for ( double rou = 0.0; rou <= ( params.imageWidth( ) + params.imageHeight( ) ) / 2; rou += 0.1 )
        {
            double rou_pow_k = 1.0;
            double z         = 0.0;

            for ( int k = 0; k < SCARAMUZZA_POLY_SIZE; k++ )
            {
                z += rou_pow_k * params.poly( k );
                rou_pow_k *= rou;
            }

            rou_vec.push_back( rou );
            z_vec.push_back( z );
        }

        assert( rou_vec.size( ) == z_vec.size( ) );
        Eigen::VectorXd xVec( rou_vec.size( ) );
        Eigen::VectorXd yVec( rou_vec.size( ) );

        for ( size_t i = 0; i < rou_vec.size( ); ++i )
        {
            xVec( i ) = std::atan2( -z_vec.at( i ), rou_vec.at( i ) );
            yVec( i ) = rou_vec.at( i );
        }

        // use lower order poly to eliminate over-fitting cause by
        // noisy/inaccurate data
        const int poly_fit_order       = 4;
        Eigen::VectorXd inv_poly_coeff = polyfit( xVec, yVec, poly_fit_order );

        for ( int i = 0; i <= poly_fit_order; ++i )
        {
            params.inv_poly( i ) = inv_poly_coeff( i );
        }
    }

    setParameters( params );

    std::cout << "initial params:\n" << params << std::endl;
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param p image coordinates
 * \param P coordinates of the point on the sphere
 */
void
OCAMCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const
{
    liftProjective( p, P );
    P.normalize( );
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param p image coordinates
 * \param P coordinates of the projective ray
 */
void
OCAMCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const
{
    // Relative to Center
    Eigen::Vector2d xc( p[0] - mParameters.center_x( ), p[1] - mParameters.center_y( ) );

    // Affine Transformation
    // xc_a = inv(A) * xc;
    Eigen::Vector2d xc_a( m_inv_scale * ( xc[0] - mParameters.D( ) * xc[1] ),
                          m_inv_scale * ( -mParameters.E( ) * xc[0] + mParameters.C( ) * xc[1] ) );

    double phi   = std::sqrt( xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1] );
    double phi_i = 1.0;
    double z     = 0.0;

    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
    {
        z += phi_i * mParameters.poly( i );
        phi_i *= phi;
    }

    P << xc[0], xc[1], -z;
}

void
OCAMCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const
{
    Eigen::Vector2d p_tmp = p / image_scale; // p_tmp is without resize, p is with resize
    liftProjective( p_tmp, P );              // p_tmp is without resize
}

/**
 * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param P 3D point coordinates
 * \param p return value, contains the image point coordinates
 */
void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const
{
    double norm    = std::sqrt( P[0] * P[0] + P[1] * P[1] );
    double theta   = std::atan2( -P[2], norm );
    double rho     = 0.0;
    double theta_i = 1.0;

    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
    {
        rho += theta_i * mParameters.inv_poly( i );
        theta_i *= theta;
    }

    double invNorm = 1.0 / norm;
    Eigen::Vector2d xn( P[0] * invNorm * rho, P[1] * invNorm * rho );

    p << xn[0] * mParameters.C( ) + xn[1] * mParameters.D( ) + mParameters.center_x( ),
    xn[0] * mParameters.E( ) + xn[1] + mParameters.center_y( );
}

void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const
{
    Eigen::Vector2d p_tmp;
    spaceToPlane( P, p_tmp );
    p = p_tmp * image_scalse;
}

void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix< double, 2, 3 >& J ) const
{
    double norm    = std::sqrt( P[0] * P[0] + P[1] * P[1] );
    double theta   = std::atan2( -P[2], norm );
    double rho     = 0.0;
    double theta_i = 1.0;

    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
    {
        rho += theta_i * mParameters.inv_poly( i );
        theta_i *= theta;
    }

    double invNorm = 1.0 / norm;
    Eigen::Vector2d xn( P[0] * invNorm * rho, P[1] * invNorm * rho );

    p << xn[0] * mParameters.C( ) + xn[1] * mParameters.D( ) + mParameters.center_x( ),
    xn[0] * mParameters.E( ) + xn[1] + mParameters.center_y( );
}
/**
 * \brief Projects an undistorted 2D point p_u to the image plane
 *
 * \param p_u 2D point coordinates
 * \return image point coordinates
 */
void
OCAMCamera::undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const
{
    Eigen::Vector3d P( p_u[0], p_u[1], 1.0 );
    spaceToPlane( P, p );
}

void
OCAMCamera::initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale ) const
{
    //    cv::Size imageSize(mParameters.imageWidth(),
    //    mParameters.imageHeight());

    //    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
    //    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);

    //    for (int v = 0; v < imageSize.height; ++v)
    //    {
    //        for (int u = 0; u < imageSize.width; ++u)
    //        {
    //            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
    //            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;

    //            double xi = mParameters.xi();
    //            double d2 = mx_u * mx_u + my_u * my_u;

    //            Eigen::Vector3d P;
    //            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0
    //            - xi * xi) * d2));

    //            Eigen::Vector2d p;
    //            spaceToPlane(P, p);

    //            mapX.at<float>(v,u) = p(0);
    //            mapY.at<float>(v,u) = p(1);
    //        }
    //    }

    //    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
}

cv::Mat
OCAMCamera::initUndistortRectifyMap(
cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat ) const
{
    if ( imageSize == cv::Size( 0, 0 ) )
    {
        imageSize = cv::Size( mParameters.imageWidth( ), mParameters.imageHeight( ) );
    }

    cv::Mat mapX = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F );
    cv::Mat mapY = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F );

    Eigen::Matrix3f K_rect;

    K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy, cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1;

    if ( fx < 0 || fy < 0 )
    {
        throw std::string( std::string( __FUNCTION__ ) + ": Focal length must be specified" );
    }

    Eigen::Matrix3f K_rect_inv = K_rect.inverse( );

    Eigen::Matrix3f R, R_inv;
    cv::cv2eigen( rmat, R );
    R_inv = R.inverse( );

    for ( int v = 0; v < imageSize.height; ++v )
    {
        for ( int u = 0; u < imageSize.width; ++u )
        {
            Eigen::Vector3f xo;
            xo << u, v, 1;

            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;

            Eigen::Vector2d p;
            spaceToPlane( uo.cast< double >( ), p );

            mapX.at< float >( v, u ) = p( 0 );
            mapY.at< float >( v, u ) = p( 1 );
        }
    }

    cv::convertMaps( mapX, mapY, map1, map2, CV_32FC1, false );

    cv::Mat K_rect_cv;
    cv::eigen2cv( K_rect, K_rect_cv );
    return K_rect_cv;
}

int
OCAMCamera::parameterCount( void ) const
{
    return SCARAMUZZA_CAMERA_NUM_PARAMS;
}

const OCAMCamera::Parameters&
OCAMCamera::getParameters( void ) const
{
    return mParameters;
}

void
OCAMCamera::setParameters( const OCAMCamera::Parameters& parameters )
{
    mParameters = parameters;

    m_inv_scale = 1.0 / ( parameters.C( ) - parameters.D( ) * parameters.E( ) );
}

void
OCAMCamera::readParameters( const std::vector< double >& parameterVec )
{
    if ( ( int )parameterVec.size( ) != parameterCount( ) )
    {
        return;
    }

    Parameters params = getParameters( );

    params.C( )        = parameterVec.at( 0 );
    params.D( )        = parameterVec.at( 1 );
    params.E( )        = parameterVec.at( 2 );
    params.center_x( ) = parameterVec.at( 3 );
    params.center_y( ) = parameterVec.at( 4 );
    for ( int i          = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        params.poly( i ) = parameterVec.at( 5 + i );
    for ( int i              = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        params.inv_poly( i ) = parameterVec.at( 5 + SCARAMUZZA_POLY_SIZE + i );

    setParameters( params );
}

void
OCAMCamera::writeParameters( std::vector< double >& parameterVec ) const
{
    parameterVec.resize( parameterCount( ) );
    parameterVec.at( 0 ) = mParameters.C( );
    parameterVec.at( 1 ) = mParameters.D( );
    parameterVec.at( 2 ) = mParameters.E( );
    parameterVec.at( 3 ) = mParameters.center_x( );
    parameterVec.at( 4 ) = mParameters.center_y( );
    for ( int i                  = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        parameterVec.at( 5 + i ) = mParameters.poly( i );
    for ( int i                                         = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        parameterVec.at( 5 + SCARAMUZZA_POLY_SIZE + i ) = mParameters.inv_poly( i );
}

void
OCAMCamera::writeParametersToYamlFile( const std::string& filename ) const
{
    mParameters.writeToYamlFile( filename );
}

std::string
OCAMCamera::parametersToString( void ) const
{
    std::ostringstream oss;
    oss << mParameters;

    return oss.str( );
}
}