/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 *************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_basegfx.hxx"
#include <osl/diagnose.h>

#include <basegfx/curve/b2dcubicbezier.hxx>

#include <basegfx/tools/debugplotter.hxx>
#include <boost/bind.hpp>

namespace basegfx
{
    namespace
    {
        void outputHeader( const ::rtl::OString& rTitle,
                           ::std::ostream* 		 pStm )
        {
            // output gnuplot setup
            if( pStm )
            {
                *pStm << "#!/usr/bin/gnuplot -persist" << ::std::endl <<
                    "#" << ::std::endl <<
                    "# automatically generated by basegfx, don't change!" << ::std::endl <<
                    "#" << ::std::endl <<
                    "#   --- " << rTitle.getStr() << " ---" << ::std::endl <<
                    "#" << ::std::endl <<
                    "set parametric" << ::std::endl <<
                    "# set terminal postscript eps enhanced color " << ::std::endl <<
                    "# set output \"plot.eps\"" << ::std::endl <<
                    // This function plots a cubic bezier curve. P,q,r,s
                    // are the control point elements of the corresponding
                    // output coordinate component (i.e. x components for
                    // the x plot, and y components for the y plot)
                    "cubicBezier(p,q,r,s,t) = p*(1-t)**3+q*3*(1-t)**2*t+r*3*(1-t)*t**2+s*t**3" << ::std::endl <<
                    // This function plots the derivative of a cubic
                    // bezier curve. P,q,r,s are the control point
                    // components of the _original_ curve
                    "cubicBezDerivative(p,q,r,s,t) = 3*(q-p)*(1-t)**2+6*(r-q)*(1-t)*t+3*(s-r)*t**2" << ::std::endl <<
                    // Plot a line's component of a line between a and b
                    // (where a and b should be the corresponding
                    // components of the line's start and end point,
                    // respectively)
                    "line(p,q,r) = p*(1-t)+q*t" << ::std::endl <<
                    // Plot a line's x component of a line in implicit
                    // form ax + by + c = 0
                    "implicitLineX(a,b,c,t) = a*-c + t*-b" << ::std::endl <<
                    // Plot a line's y component of a line in implicit
                    // form ax + by + c = 0
                    "implicitLineY(a,b,c,t) = b*-c + t*a" << ::std::endl <<
                    "pointmarkx(c,t) = c-0.03*t" << ::std::endl <<										 // hack for displaying single points in parametric form
                    "pointmarky(c,t) = c+0.03*t" << ::std::endl <<										 // hack for displaying single points in parametric form
                    "# end of setup" << ::std::endl;
            }
            else
            {
                OSL_TRACE( "#!/usr/bin/gnuplot -persist\n",
                           "#\n",
                           "# automatically generated by basegfx, don't change!\n",
                           "#\n",
                           "#   --- %s ---\n",
                           "#\n",
                           "set parametric\n",
                           // This function plots a cubic bezier curve. P,q,r,s
                           // are the control point elements of the corresponding
                           // output coordinate component (i.e. x components for
                           // the x plot, and y components for the y plot)
                           "cubicBezier(p,q,r,s,t) = p*(1-t)**3+q*3*(1-t)**2*t+r*3*(1-t)*t**2+s*t**3\n",
                           // This function plots the derivative of a cubic
                           // bezier curve. P,q,r,s are the control point
                           // components of the _original_ curve
                           "cubicBezDerivative(p,q,r,s,t) = 3*(q-p)*(1-t)**2+6*(r-q)*(1-t)*t+3*(s-r)*t**2\n",
                           // Plot a line's component of a line between a and b
                           // (where a and b should be the corresponding
                           // components of the line's start and end point,
                           // respectively)
                           "line(p,q,r) = p*(1-t)+q*t\n",
                           // Plot a line's x component of a line in implicit
                           // form ax + by + c = 0
                           "implicitLineX(a,b,c,t) = a*-c + t*-b\n",
                           // Plot a line's y component of a line in implicit
                           // form ax + by + c = 0
                           "implicitLineY(a,b,c,t) = b*-c + t*a\n",
                           "pointmarkx(c,t) = c-0.03*t\n",										 // hack for displaying single points in parametric form
                           "pointmarky(c,t) = c+0.03*t\n",										 // hack for displaying single points in parametric form
                           "# end of setup\n",
                           rTitle.getStr() );
            }
        }

        class Writer
        {
        public:
            Writer( ::std::ostream* pStm ) :
                mpStream( pStm )
            {
            }

            void outputPoint( const ::std::pair< B2DPoint, ::rtl::OString >& rElem )
            {
                if( mpStream )
                    *mpStream << " " << rElem.first.getX() << "\t" << rElem.first.getY() << ::std::endl;
                else
                    OSL_TRACE( " %f\t%f\n", rElem.first.getX(), rElem.first.getY() );
            }

            void outputVector( const ::std::pair< B2DVector, ::rtl::OString >& rElem )
            {
                if( mpStream )
                    *mpStream << " " << rElem.first.getX() << "\t" << rElem.first.getY() << ::std::endl << ::std::endl;
                else
                    OSL_TRACE( " %f\t%f\n\n", rElem.first.getX(), rElem.first.getY() );
            }

            void outputRect( const ::std::pair< B2DRange, ::rtl::OString >& rElem )
            {
                const double nX0( rElem.first.getMinX() );
                const double nY0( rElem.first.getMinY() );
                const double nX1( rElem.first.getMaxX() );
                const double nY1( rElem.first.getMaxY() );

                if( mpStream )
                    *mpStream << " "
                              << nX0 << "\t" << nY0 << "\t"
                              << nX1 << "\t" << nY0 << "\t"
                              << nX1 << "\t" << nY1 << "\t"
                              << nX0 << "\t" << nY1 << "\t"
                              << nX0 << "\t" << nY0 << ::std::endl << ::std::endl;

                else
                    OSL_TRACE( " %f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n\n",
                               nX0, nY0,
                               nX1, nY0,
                               nX1, nY1,
                               nX0, nY1,
                               nX0, nY0 );
            }

        private:
            ::std::ostream* 	mpStream;
        };
    }

    DebugPlotter::DebugPlotter( const sal_Char* pTitle ) :
        maTitle( pTitle ),
        maPoints(),
        maVectors(),
        maRanges(),
        maPolygons(),
        mpOutputStream(NULL)
    {
    }

    DebugPlotter::DebugPlotter( const sal_Char* pTitle,
                                ::std::ostream& rOutputStream ) :
        maTitle( pTitle ),
        maPoints(),
        maVectors(),
        maRanges(),
        maPolygons(),
        mpOutputStream(&rOutputStream)
    {
    }

    DebugPlotter::~DebugPlotter()
    {
        const bool bHavePoints( !maPoints.empty() );
        const bool bHaveVectors( !maVectors.empty() );
        const bool bHaveRanges( !maRanges.empty() );
        const bool bHavePolygons( !maPolygons.empty() );

        if( bHavePoints ||
            bHaveVectors ||
            bHaveRanges ||
            bHavePolygons )
        {
            outputHeader( maTitle, mpOutputStream );

            print( "\n\n# parametric primitive output\n"
                   "plot [t=0:1] \\\n" );

            // output plot declarations for used entities
            bool bNeedColon( false );
            if( bHavePoints )
            {
                print( " '-' using ($1):($2) title \"Points\" with points" );
                bNeedColon = true;
            }
            if( bHaveVectors )
            {
                if( bNeedColon )
                    print( ", \\\n" );

                print( " '-' using ($1):($2) title \"Vectors\" with lp" );
                bNeedColon = true;
            }
            if( bHaveRanges )
            {
                if( bNeedColon )
                    print( ", \\\n" );

                print( " '-' using ($1):($2) title \"Ranges\" with lines" );
                bNeedColon = true;
            }
            if( bHavePolygons )
            {
                const ::std::size_t nSize( maPolygons.size() );
                for( ::std::size_t i=0; i<nSize; ++i )
                {
                    if( maPolygons.at(i).first.areControlPointsUsed() )
                    {
                        const B2DPolygon& rCurrPoly( maPolygons.at(i).first );

                        const sal_uInt32 nCount( rCurrPoly.count() );
                        for( sal_uInt32 k=0; k<nCount; ++k )
                        {
                            if( bNeedColon )
                                print( ", \\\n" );

                            const B2DPoint& rP0( rCurrPoly.getB2DPoint(k) );
                            const B2DPoint& rP1( rCurrPoly.getNextControlPoint(k) );
                            const B2DPoint& rP2( rCurrPoly.getPrevControlPoint((k + 1) % nCount) );
                            const B2DPoint& rP3( k+1<nCount ? rCurrPoly.getB2DPoint(k+1) : rCurrPoly.getB2DPoint(k) );

                            if( mpOutputStream )
                                *mpOutputStream << "  cubicBezier("
                                                << rP0.getX() << ","
                                    << rP1.getX() << ","
                                    << rP2.getX() << ","
                                    << rP3.getX() << ",t), \\\n   cubicBezier("
                                    << rP0.getY() << ","
                                    << rP1.getY() << ","
                                    << rP2.getY() << ","
                                    << rP3.getY() << ",t)";
                            else
                                OSL_TRACE( "  cubicBezier(%f,%f,%f,%f,t), \\\n"
                                           "   cubicBezier(%f,%f,%f,%f,t)",
                                           rP0.getX(),
                                           rP1.getX(),
                                           rP2.getX(),
                                           rP3.getX(),
                                           rP0.getY(),
                                           rP1.getY(),
                                           rP2.getY(),
                                           rP3.getY() );

                            bNeedColon = true;
                        }
                    }
                    else
                    {
                        if( bNeedColon )
                            print( ", \\\n" );

                        if( mpOutputStream )
                            *mpOutputStream << " '-' using ($1):($2) title \"Polygon "
                                            << maPolygons.at(i).second.getStr() << "\" with lp";
                        else
                            OSL_TRACE( " '-' using ($1):($2) title \"Polygon %s\" with lp",
                                       maPolygons.at(i).second.getStr() );

                        bNeedColon = true;
                    }
                }
            }

            if( bHavePoints )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maPoints.begin(),
                                 maPoints.end(),
                                 ::boost::bind( &Writer::outputPoint,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHaveVectors )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maVectors.begin(),
                                 maVectors.end(),
                                 ::boost::bind( &Writer::outputVector,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHaveRanges )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maRanges.begin(),
                                 maRanges.end(),
                                 ::boost::bind( &Writer::outputRect,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHavePolygons )
            {
                const ::std::size_t nSize( maPolygons.size() );
                for( ::std::size_t i=0; i<nSize; ++i )
                {
                    if( !maPolygons.at(i).first.areControlPointsUsed() )
                    {
                        const B2DPolygon& rCurrPoly( maPolygons.at(i).first );

                        const sal_uInt32 nCount( rCurrPoly.count() );
                        for( sal_uInt32 k=0; k<nCount; ++k )
                        {
                            const B2DPoint& rP( rCurrPoly.getB2DPoint(k) );

                            if( mpOutputStream )
                                *mpOutputStream << " " << rP.getX() << "," << rP.getY();
                            else
                                OSL_TRACE( " %f,%f",
                                           rP.getX(),
                                           rP.getX() );
                        }

                        print( "\ne\n" );
                    }
                }
            }
        }
    }

    void DebugPlotter::plot( const B2DPoint& rPoint,
                             const sal_Char* pTitle )
    {
        maPoints.push_back( ::std::make_pair( rPoint,
                                              ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DVector&	rVec,
                             const sal_Char* 	pTitle )
    {
        maVectors.push_back( ::std::make_pair( rVec,
                                               ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DCubicBezier&	rBezier,
                             const sal_Char* 		pTitle )
    {
        B2DPolygon aPoly;
        aPoly.append(rBezier.getStartPoint());
		aPoly.appendBezierSegment(rBezier.getControlPointA(), rBezier.getControlPointB(), rBezier.getEndPoint());
        maPolygons.push_back( ::std::make_pair( aPoly,
                                                ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DRange& rRange,
                             const sal_Char* pTitle )
    {
        maRanges.push_back( ::std::make_pair( rRange,
                                              ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DPolygon&	rPoly,
                             const sal_Char* 	pTitle )
    {
        maPolygons.push_back( ::std::make_pair( rPoly,
                                                ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DPolyPolygon&	rPoly,
                             const sal_Char* 		pTitle )
    {
        const ::rtl::OString aTitle( pTitle );
        const sal_uInt32 nCount( rPoly.count() );
        for( sal_uInt32 i=0; i<nCount; ++i )
            maPolygons.push_back( ::std::make_pair( rPoly.getB2DPolygon( i ),
                                                    aTitle ) );
    }

    void DebugPlotter::print( const sal_Char* pStr )
    {
        if( mpOutputStream )
            *mpOutputStream << pStr;
        else
            OSL_TRACE( pStr );
    }
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * 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 The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "gtest/gtest.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "theia/math/util.h"
#include "theia/sfm/camera/camera.h"
#include "theia/sfm/estimators/camera_and_feature_correspondence_2d_3d.h"
#include "theia/sfm/estimators/estimate_similarity_transformation_2d_3d.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/similarity_transformation.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"

namespace theia {

static const double kFocalLength = 1000.0;
static const double kReprojectionError = 4.0;
static const int kNumCameras = 100;

RandomNumberGenerator rng(154);

inline Camera RandomCamera() {
  Camera camera;
  camera.SetPosition(rng.RandVector3d(-10.0, 10.0));
  camera.SetOrientationFromRotationMatrix(RandomRotation(10.0, &rng));
  camera.SetImageSize(1000, 1000);
  camera.SetFocalLength(kFocalLength);
  camera.SetPrincipalPoint(kFocalLength / 2.0, kFocalLength / 2.0);
  return camera;
}

void ExecuteRandomTest(
    const RansacParameters& params,
    const SimilarityTransformation& similarity_transformation,
    const double inlier_ratio,
    const double noise,
    const double tolerance) {
  std::vector<CameraAndFeatureCorrespondence2D3D> correspondences;
  for (int i = 0; i < kNumCameras; i++) {
    CameraAndFeatureCorrespondence2D3D correspondence;

    // Set up random camera.
    correspondence.camera = RandomCamera();

    // Set up random 3D point and reproject it into the image. Make sure the
    // point is in front of the camera.
    double depth = -1;
    do {
      // Create a 3D point randomly, and try to ensure that it is in front of
      // the camera.
      correspondence.point3d = Eigen::Vector4d(rng.RandDouble(-5, 5),
                                               rng.RandDouble(-5, 5),
                                               rng.RandDouble(10, 20),
                                               1.0);

      depth = correspondence.camera.ProjectPoint(
          correspondence.point3d, &correspondence.observation.point_);
    } while (depth < 0);
    correspondences.emplace_back(std::move(correspondence));
  }

  // Add noise to the image observations.
  if (noise) {
    for (int i = 0; i < kNumCameras; i++) {
      correspondences[i].observation.point_ += noise * rng.RandVector2d();
    }
  }

  // Add outliers.
  for (int i = 0; i < kNumCameras; i++) {
    if (i > inlier_ratio * kNumCameras) {
      correspondences[i].observation.point_ = kFocalLength * rng.RandVector2d();
    }
  }

  // Apply the similarity transformation to the 3d points.
  for (int i = 0; i < kNumCameras; i++) {
    const Eigen::Vector3d old_point = correspondences[i].point3d.hnormalized();
    const Eigen::Vector3d new_point = similarity_transformation.scale *
                                          similarity_transformation.rotation *
                                          old_point +
                                      similarity_transformation.translation;
    correspondences[i].point3d = new_point.homogeneous();
  }

  // Estimate the similarity transformation;
  SimilarityTransformation estimated_similarity_transformation;
  RansacSummary ransac_summary;
  EXPECT_TRUE(
      EstimateSimilarityTransformation2D3D(params,
                                           RansacType::RANSAC,
                                           correspondences,
                                           &estimated_similarity_transformation,
                                           &ransac_summary));

  // We should have found at least one good solution.
  EXPECT_GT(ransac_summary.inliers.size(), 4);

  // Expect poses are near.
  EXPECT_TRUE(test::ArraysEqualUpToScale(
      9,
      estimated_similarity_transformation.rotation.data(),
      similarity_transformation.rotation.data(),
      tolerance));
  EXPECT_TRUE(test::ArraysEqualUpToScale(
      3,
      estimated_similarity_transformation.translation.data(),
      similarity_transformation.translation.data(),
      tolerance));
  EXPECT_NEAR(estimated_similarity_transformation.scale,
              similarity_transformation.scale,
              10 * tolerance);
}

TEST(EstimateSimilarityTransformation2D3D, AllInliersNoNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.001;
  options.max_iterations = 1000;
  const double kInlierRatio = 1.0;
  const double kNoise = 0.0;
  const double kPoseTolerance = 1e-4;

  SimilarityTransformation similarity_transformation;
  similarity_transformation.rotation =
      Eigen::AngleAxisd(DegToRad(12.0),
                        Eigen::Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix();
  similarity_transformation.translation = Eigen::Vector3d(-1.3, 2.1, 0.5);
  similarity_transformation.scale = 0.8;
  ExecuteRandomTest(
      options, similarity_transformation, kInlierRatio, kNoise, kPoseTolerance);
}

TEST(EstimateSimilarityTransformation2D3D, AllInliersWithNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.001;
  options.max_iterations = 1000;
  const double kInlierRatio = 1.0;
  const double kNoise = 1.0;
  const double kPoseTolerance = 1e-2;

  SimilarityTransformation similarity_transformation;
  similarity_transformation.rotation =
      Eigen::AngleAxisd(DegToRad(12.0),
                        Eigen::Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix();
  similarity_transformation.translation = Eigen::Vector3d(-1.3, 2.1, 0.5);
  similarity_transformation.scale = 0.8;
  ExecuteRandomTest(
      options, similarity_transformation, kInlierRatio, kNoise, kPoseTolerance);
}

TEST(EstimateSimilarityTransformation2D3D, OutliersNoNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.001;
  options.max_iterations = 1000;
  const double kInlierRatio = 0.8;
  const double kNoise = 0.0;
  const double kPoseTolerance = 1e-4;

  SimilarityTransformation similarity_transformation;
  similarity_transformation.rotation =
      Eigen::AngleAxisd(DegToRad(12.0),
                        Eigen::Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix();
  similarity_transformation.translation = Eigen::Vector3d(-1.3, 2.1, 0.5);
  similarity_transformation.scale = 0.8;
  ExecuteRandomTest(
      options, similarity_transformation, kInlierRatio, kNoise, kPoseTolerance);
}

TEST(EstimateSimilarityTransformation2D3D, OutliersWithNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.001;
  options.max_iterations = 1000;
  const double kInlierRatio = 0.8;
  const double kNoise = 1.0;
  const double kPoseTolerance = 1e-2;

  SimilarityTransformation similarity_transformation;
  similarity_transformation.rotation =
      Eigen::AngleAxisd(DegToRad(12.0),
                        Eigen::Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix();
  similarity_transformation.translation = Eigen::Vector3d(-1.3, 2.1, 0.5);
  similarity_transformation.scale = 0.8;
  ExecuteRandomTest(
      options, similarity_transformation, kInlierRatio, kNoise, kPoseTolerance);
}

}  // namespace theia


///////////////////////////////////////////////////////////////////////////////
// sample.hpp                                                                //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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 BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_SAMPLE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_SAMPLE_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/sample/view.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/sample/hash.hpp>
//#include <boost/statistics/detail/non_parametric/contingency_table/sample/check_against_domain.hpp>
//#include <boost/statistics/detail/non_parametric/contingency_table/sample/domain_error_logger.hpp>

#endif


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS 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.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#define TEST_NAME "base/projection"
#include "util/testing.h"

#include <Eigen/Core>

#include "base/camera_models.h"
#include "base/pose.h"
#include "base/projection.h"
#include "util/math.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestComposeProjectionMatrix) {
  const Eigen::Matrix3d R = EulerAnglesToRotationMatrix(0, 1, 2);
  const Eigen::Vector4d qvec = RotationMatrixToQuaternion(R);
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix1 = ComposeProjectionMatrix(qvec, tvec);
  const auto proj_matrix2 = ComposeProjectionMatrix(R, tvec);

  BOOST_CHECK((proj_matrix1 - proj_matrix2).norm() < 1e-6);
  BOOST_CHECK((proj_matrix1.leftCols<3>() - R).norm() < 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix1.rightCols<1>() - tvec).norm(), 0, 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix2.leftCols<3>() - R).norm(), 0, 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix2.rightCols<1>() - tvec).norm(), 0, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestInvertProjectionMatrix) {
  const Eigen::Matrix3d R = EulerAnglesToRotationMatrix(0, 1, 2);
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix = ComposeProjectionMatrix(R, tvec);
  const auto inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
  const auto inv_inv_proj_matrix = InvertProjectionMatrix(inv_proj_matrix);

  BOOST_CHECK((proj_matrix - inv_inv_proj_matrix).norm() < 1e-6);
}

BOOST_AUTO_TEST_CASE(TestComputeClosestRotationMatrix) {
  const Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
  BOOST_CHECK_LT((ComputeClosestRotationMatrix(A) - A).norm(), 1e-6);
  BOOST_CHECK_LT((ComputeClosestRotationMatrix(2 * A) - A).norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestDecomposeProjectionMatrix) {
  for (int i = 1; i < 100; ++i) {
    Eigen::Matrix3d ref_K = i * Eigen::Matrix3d::Identity();
    ref_K(0, 2) = i;
    ref_K(1, 2) = 2 * i;
    const Eigen::Matrix3d ref_R = EulerAnglesToRotationMatrix(i, 2 * i, 3 * i);
    const Eigen::Vector3d ref_T = Eigen::Vector3d::Random();
    const Eigen::Matrix3x4d ref_P =
        ref_K * ComposeProjectionMatrix(ref_R, ref_T);
    Eigen::Matrix3d K;
    Eigen::Matrix3d R;
    Eigen::Vector3d T;
    DecomposeProjectionMatrix(ref_P, &K, &R, &T);
    BOOST_CHECK(ref_K.isApprox(K, 1e-6));
    BOOST_CHECK(ref_R.isApprox(R, 1e-6));
    BOOST_CHECK(ref_T.isApprox(T, 1e-6));
  }
}

BOOST_AUTO_TEST_CASE(TestCalculateReprojectionError) {
  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  const Eigen::Vector3d point3D = Eigen::Vector3d::Random();
  const Eigen::Vector3d point2D_h = proj_matrix * point3D.homogeneous();
  const Eigen::Vector2d point2D = point2D_h.hnormalized();

  Camera camera;
  camera.InitializeWithId(SimplePinholeCameraModel::model_id, 1, 0, 0);

  const double error1 =
      CalculateReprojectionError(point2D, point3D, proj_matrix, camera);
  BOOST_CHECK_CLOSE(error1, 0, 1e-6);

  const double error2 = CalculateReprojectionError(point2D.array() + 1, point3D,
                                                   proj_matrix, camera);
  BOOST_CHECK_CLOSE(error2, std::sqrt(2), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestCalculateAngularError) {
  const Eigen::Vector4d qvec = ComposeIdentityQuaternion();
  const Eigen::Vector3d tvec = Eigen::Vector3d(0, 0, 0);

  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);
  Camera camera;
  camera.SetModelId(SimplePinholeCameraModel::model_id);
  camera.Params() = {1, 0, 0};

  const double error1 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error1, 0, 1e-6);

  const double error2 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 1, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error2, M_PI / 4, 1e-6);

  const double error3 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 5, 5), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error3, M_PI / 4, 1e-6);

  const double error4 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error4, M_PI / 4, 1e-6);

  const double error5 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error5, 1.10714872, 1e-6);

  const double error6 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(1, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error6, 1.10714872 - M_PI / 4, 1e-6);

  const double error7 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(5, 0, 5), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error7, 1.10714872 - M_PI / 4, 1e-6);

  const double error8 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error8, M_PI / 2, 1e-6);

  const double error9 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, 0), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error9, M_PI * 3 / 4, 1e-6);

  const double error10 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, -1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error10, M_PI, 1e-6);

  const double error11 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(0, 0, -1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error11, M_PI * 3 / 4, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestCalculateDepth) {
  const Eigen::Vector4d qvec(1, 0, 0, 0);
  const Eigen::Vector3d tvec(0, 0, 0);
  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  // In the image plane
  const double depth1 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK_CLOSE(depth1, 0, 1e-10);
  const double depth2 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 2, 0));
  BOOST_CHECK_CLOSE(depth2, 0, 1e-10);

  // Infront of camera
  const double depth3 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, 1));
  BOOST_CHECK_CLOSE(depth3, 1, 1e-10);

  // Behind camera
  const double depth4 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, -1));
  BOOST_CHECK_CLOSE(depth4, -1, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestHasPointPositiveDepth) {
  const Eigen::Vector4d qvec(1, 0, 0, 0);
  const Eigen::Vector3d tvec(0, 0, 0);
  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  // In the image plane
  const bool check1 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK(!check1);
  const bool check2 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 2, 0));
  BOOST_CHECK(!check2);

  // Infront of camera
  const bool check3 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, 1));
  BOOST_CHECK(check3);

  // Behind camera
  const bool check4 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, -1));
  BOOST_CHECK(!check4);
}


/*!
 * @file
 * This file defines `boost::mpl::topological_sort`.
 */

#ifndef BOOST_MPL_TOPOLOGICAL_SORT_HPP
#define BOOST_MPL_TOPOLOGICAL_SORT_HPP

#include <boost/mpl/front_inserter.hpp>
#include <boost/mpl/graph_intrinsics.hpp>
#include <boost/mpl/is_reachable.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/sort.hpp>
#include <boost/mpl/vector.hpp>

namespace boost { namespace mpl {
    //! Return a valid topological ordering of the vertices of a graph.
    template <typename Graph>
    struct topological_sort
        : sort<typename vertices_of<Graph>::type,
            is_reachable<Graph, _1, _2>,
            front_inserter<typename vector<>::type>
        >
    { };
}} // end namespace boost::mpl

#endif // !BOOST_MPL_TOPOLOGICAL_SORT_HPP


#ifndef SPECIAL_HPP
#define SPECIAL_HPP

#include <boost/shared_ptr.hpp>
#include "../workspace.hpp"
#include <cmath>
#include <memory>

namespace dqmc {
    namespace la {

#ifdef USE_DD
	inline void copy_to_dd(pmat_t& in, dd_mat& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    out(r, c) = ddouble(in(r, c));
		}
	    }
	}

	inline void copy_to_dd(pvec_t& in, dd_vec& out) {
	    for (int c = 0; c < in.size(); ++c) {
		out(c) = ddouble(in(c));
	    }
	}

	inline void copy_from_dd(dd_mat& in, pmat_t& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    out(r, c) = to_double(in(r, c));
		}
	    }
	}

	inline void copy_from_dd(dd_vec& in, pvec_t& out) {
	    for (int c = 0; c < in.rows(); ++c) {
		out(c) = to_double(in(c));
	    }
	}

	inline void dd_normalize(dd_vec&__restrict__ v) {
	    dd_real norm = 0;

	    for (int i = 0; i < v.rows() * v.cols(); ++i) {
		if ((v(i) * v(i)).isnan()) {
		    std::cout << v(i) << " squared is nan" << std::endl;
		    throw std::runtime_error("error");
		}
		norm += v(i) * v(i);
	    }

	    v /= sqrt(norm);
	}

	inline void dd_dot(dd_vec&__restrict__ v1, dd_vec&__restrict__ v2,
			   dd_real&__restrict__ result) {
	    result = dd_real(0, 0);

	    for (int i = 0; i < v1.rows() * v1.cols(); ++i) {
		if ((v1(i) * v2(i)).isnan()) {
		    std::cout << v1(i) << " times " << v2(i) << " is nan" << std::endl;
		    throw std::runtime_error("error");
		}
		result += v1(i) * v2(i);
	    }
	}

	inline void dd_from_col(int c, const dd_mat& in, dd_vec& out) {
	    for (int r = 0; r < in.rows(); ++r) {
		out(r) = in(r, c);
	    }
	}

	inline void dd_to_col(int c, const dd_vec& in, dd_mat& out) {
	    for (int r = 0; r < in.rows(); ++r) {
		out(r, c) = in(r);
	    }
	}

	inline void dd_from_row(int r, const dd_mat& in, dd_vec& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		out(c) = in(r, c);
	    }
	}

	inline void dd_to_row(int r, const dd_vec& in, dd_mat& out) {
	    for (int c = 0; c < out.cols(); ++c) {
		out(r, c) = in(c);
	    }
	}

	inline void dd_thin_col_to_invertible(dd_mat& in, dd_mat& temp, dd_mat& out,
					      dd_vec& t_vec_1, dd_vec& t_vec_2) {
	    using namespace std;

	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }

	    for (int c = in.cols(); c < out.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = ddrand();// / dd_real::_max;
		}
	    }

	    // cout << "Randomized full col" << endl;
	    // dqmc::la::print_matrix(temp.rows(), temp.cols(), temp);
	    // Gram-Schmidt orthogonalization
	    dd_real projection;

	    for (int iter = 0; iter < 5; ++iter) {
		// cout << "First iteration" << endl;

		for (int c = in.cols(); c < out.cols(); ++c) {
		    // cout << "from_col" << endl;
		    dd_from_col(c, temp, t_vec_1);

		    for (int c2 = 0; c2 < c; ++c2) {
			// cout << "c2" << endl;
			dd_from_col(c2, out, t_vec_2);
			// cout << "dd_dot" << endl;
			dd_dot(t_vec_1, t_vec_2, projection);
			// cout << projection << endl;
			t_vec_1 -= projection * t_vec_2;
		    }

		    dd_normalize(t_vec_1);
		    dd_to_col(c, t_vec_1, out);
		}
		temp = out;
	    }
	    // dqmc::la::print_matrix(out.rows(), out.cols(), out);
	}

	inline void dd_thin_row_to_invertible(dd_mat& in, dd_mat& temp, dd_mat& out,
					      dd_vec& t_vec_1, dd_vec& t_vec_2) {
	    using namespace std;

	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }

	    for (int c = in.rows(); c < out.rows(); ++c) {
		for (int r = 0; r < in.cols(); ++r) {
		    temp(r, c) = ddrand();// / dd_real::_max;
		    if (isnan(temp(r, c))) {
			cout << "Random nan???" << endl;
			throw std::runtime_error("no");
		    }
		}
	    }

	    dd_real projection;

	    for (int iter = 0; iter < 5; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    dd_from_row(r, temp, t_vec_1);

		    for (int r2 = 0; r2 < r; ++r2) {
			dd_from_row(r2, out, t_vec_2);
			dd_dot(t_vec_1, t_vec_2, projection);
			t_vec_1 -= projection * t_vec_2;
		    }

		    dd_normalize(t_vec_1);
		    dd_to_row(r, t_vec_1, out);
		}
		temp = out;
	    }
	}
#endif

	inline void randomize(pmat_t& M) {
	    for (int r = 0; r < M.rows(); r++) {
		for ( int c = 0; c < M.cols(); c++) {
		    M(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		}
	    }
	    std::srand (std::rand());
	}

	inline void normalize(pvec_t&__restrict__ v) {
	    pdouble_t norm = 0;

	    for (int i = 0; i < v.size(); ++i) {
		norm += v(i) * v(i);
	    }

	    for (int i = 0; i < v.size(); ++i) {
		v(i) /= sqrt(norm);
	    }
	}

	inline void random_with_inverse(pmat_t&__restrict__ in,
					pmat_t&__restrict__ in_inv) {
	    throw std::runtime_error("Broken");
	    // for (int c = 0; c < in.cols(); ++c) {
	    // 	for (int r = 0; r < in.rows(); ++r) {
	    // 	    in(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
	    // 	}
	    // }

	    // pmat_t U = in;
	    // pmat_t T = in;
	    // pvec_t D(in.rows());
	    // pmat_t U_inv = in;
	    // pmat_t T_inv = in;

	    // decompose_dgejsv_nt(in, U, D, T);
	    // matrix_transpose(U, T_inv);
	    // matrix_transpose(T, U_inv);
	    // thin_inv_sandwich(U_inv, D, T_inv, in_inv);
	}

	inline void random_orthogonal(pmat_t&__restrict__ in, boost::shared_ptr<dqmc::workspace> ws) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    in(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		}
	    }
	    pdouble_t projection, inner_prod;

	    pvec_t vec_1(in.rows());
	    pvec_t vec_2(in.rows());
	    pvec_t vec_3(in.rows());

	    for(int c = 0; c < in.cols(); ++c) {
		col(c, in, vec_1);
		normalize(vec_1);
		to_col(c, in, vec_1);
		dot(vec_1, vec_1, projection);
		// std::cout << "Projection of " << c << " - " << projection << std::endl;
	    }
	    // gram-Schmidt orthogonalization

	    for (int iter = 0; iter < 5; ++iter) {
		pmat_t mat_1 = in;
		for(int c = 0; c < in.cols(); ++c) {
		    col(c, in, vec_1);
		    dot(vec_1, vec_1, projection);
		    // std::cout << "Before Projection of " << c << " - " << projection << std::endl;
		}

		for (int i = 1; i < in.rows(); ++i) {
		    col(i, mat_1, vec_2);
		    col(i, mat_1, vec_3);

		    for (int j = 0; j < i; ++j) {
			col(j, mat_1, vec_1);

			dot(vec_1, vec_3, projection);
			dot(vec_1, vec_1, inner_prod);
			vec_2 -= projection/inner_prod * vec_1;
		    }
		    normalize(vec_2);
		    to_col(i, mat_1, vec_2);
		}
		in = mat_1;
		// std::cout << "Done with iter " << iter << std::endl;
		// for(int c = 0; c < in.cols(); ++c) {
		//     col(c, in, vec_1);
		//     dot(vec_1, vec_1, projection);
		//     std::cout << "Projection of " << c << " - " << projection << std::endl;
		// }

	    }
	    // for(int c = 0; c < in.cols(); ++c) {
	    // 	col(c, in, vec_1);
	    // 	dot(vec_1, vec_1, projection);
	    // 	std::cout << "Projection of " << c << " - " << projection << std::endl;
	    // }
	}

	inline void tiny_to_invertible(const pmat_t&__restrict__ in, const pmat_t& trans, pmat_t&__restrict__ out) {
	    // out.zeros();
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), in.cols(), trans.rows(), trans.cols()) = trans;
	}

	inline void tiny_plus_random(const pmat_t&__restrict__ in, pmat_t& out) {
	    // out.zeros();
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), in.cols(), out.rows() - in.rows(), out.cols() - in.cols()).setRandom();
	}

	inline void thin_col_plus_random(const pmat_t&__restrict__ in, pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(0, in.cols(), out.rows(), out.cols() - in.cols()).setRandom();
	}

	inline void thin_col_to_invertible(const pmat_t&__restrict__ in, pmat_t&__restrict__ out) {
	    using namespace std;
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(0, in.cols(), out.rows(), out.cols() - in.cols()).setRandom();

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 2; ++iter) {
		for (int c = in.cols(); c < out.cols(); ++c) {
		    for (int c2 = 0; c2 < c; ++c2) {
			projection = out.col(c).transpose() * out.col(c2);
			out.col(c) -= projection * out.col(c2);
		    }
		    out.col(c).normalize();
		}
	    }
	}

	inline void thin_col_to_invertible_old(const pmat_t&__restrict__ in, pmat_t&__restrict__ out,
					       dqmc::workspace& ws) {
	    using namespace std;
	    // out.zeros();
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }
	    // return;

	    for (int c = in.cols(); c < out.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		    // for (int o = 0; o < in.rows()*in.cols(); o++) {
		    // 	if (!(fabs(in((r * in.rows() + c * in.cols() + o) % in.rows(),
		    // 		      (r * in.cols() + c * in.rows() + o) % in.cols())) == 0)) {
		    // 	    ws.la_mat_1(r, c) = fabs(in((r * in.rows() + c * in.cols() + o) % in.rows(),
		    // 					 (r * in.cols() + c * in.rows() + o) % in.cols()));
		    // 	    break;
		    // 	}
		    // }
		    // ws.la_mat_1(r, c) = 0.;

		    if (r == c) {
			if (ws.la_mat_1(r, c) == 0) { ws.la_mat_1(r, c) = 1.;	}
			ws.la_mat_1(r, c) *= -1;
		    }
		}
	    }
	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 5; ++iter) {
		for (int c = in.cols(); c < out.cols(); ++c) {
		    //col(c, ws.la_mat_1, ws.la_vec_1);
		    ws.la_vec_1 = ws.la_mat_1.col(c);

		    for (int c2 = 0; c2 < c; ++c2) {
			// col(c2, out, ws.la_vec_2);
			ws.la_vec_2 = out.col(c2);
			// dot(ws.la_vec_1, ws.la_vec_2, projection);
			projection = ws.la_vec_1.transpose() * ws.la_vec_2;
			ws.la_vec_1 -= projection * ws.la_vec_2;
		    }
		    // normalize(ws.la_vec_1);
		    ws.la_vec_1.normalize();
		    // to_col(c, out, ws.la_vec_1);
		    out.col(c) = ws.la_vec_1;
		}
		ws.la_mat_1 = out;
	    }
	}

	inline void thin_row_plus_random(const pmat_t&__restrict__ in,
					 pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), 0, out.rows() - in.rows(), in.cols()).setRandom();
	}

	inline void thin_row_to_invertible(const pmat_t&__restrict__ in,
					   pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), 0, out.rows() - in.rows(), in.cols()).setRandom();

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 2; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    for (int r2 = 0; r2 < r; ++r2) {
			projection = out.row(r) * out.row(r2).transpose();
			out.row(r) -= projection * out.row(r2);
		    }
		    out.row(r).normalize();
		}
	    }
	}

	inline void thin_row_to_invertible_old(const pmat_t&__restrict__ in,
					       pmat_t&__restrict__ out,
					       dqmc::workspace& ws) {
	    // out.zeros();
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }
	    // return;

	    for (int c = 0; c < out.cols(); ++c) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    ws.la_mat_1(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		    if (r == c) {
			if (ws.la_mat_1(r, c) == 0) { ws.la_mat_1(r, c) = 1.;	}
			ws.la_mat_1(r, c) *= -1;
		    }
		}
	    }

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;
	    vec temp = ws.la_mat_1.row(0);

	    for (int iter = 0; iter < 5; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    row(r, ws.la_mat_1, ws.la_vec_1);
		    // temp = ws.la_mat_1.row(r);
		    for (int r2 = 0; r2 < r; ++r2) {
			// projection = ws.la_mat_1.row(r) * out.row(r2).transpose();
			row(r2, out, ws.la_vec_2);
			dot(ws.la_vec_1, ws.la_vec_2, projection);
			ws.la_vec_1 -= projection * ws.la_vec_2;
			// ws.la_mat_1.row(r) -= projection * out.row(r2);
		    }

		    // ws.la_mat_1.row(r) /= sqrt(ws.la_mat_1.row(r) * ws.la_mat_1.row(r).transpose());
		    // ws.la_mat_1.row(r).normalize();
		    normalize(ws.la_vec_1);
		    to_row(r, out, ws.la_vec_1);
		}
		ws.la_mat_1 = out;
	    }
	}

	inline pdouble_t trace(pmat_t& M) {
	    pdouble_t tr = 0.;
	    for (int i = 0; i < M.rows(); i++) {
		tr += M(i, i);
	    }
	    return tr;
	}

	inline void split_vec_sqrt(pvec_t&__restrict__ in, pvec_t&__restrict__ out) {
	    for (int i = 0; i < in.size(); ++i) {
		out(i) = sqrt(in(i));
	    }
	}

	inline bool all_below_eps(const pmat_t&__restrict M) {
	    for (int c = 0; c < M.cols(); ++c) {
		for (int r = 0; r < M.rows(); ++r) {
		    if (fabs(M(r, c)) > 1e-14) { return false; }
		}
	    }
	    return true;
	}

	inline void sum(const pvec_t&__restrict__ v, double& sum) {
	    sum = 0;
	    for (int i = 0; i < v.size(); ++i) {
		sum += v(i);
	    }}

    }
}
#endif


#include <boost/python.hpp>
#include <stdexcept>
#include <stddef.h>
#include <string>
#include "nifty_iterator.hxx"
#include "inf_def_visitor.hxx"
#include "lp_def_suite.hxx"
#include "export_typedes.hxx"

#include <opengm/inference/lp_inference.hxx>
#include <param/lp_inference_param.hxx>

#ifdef WITH_CPLEX
#include <opengm/inference/auxiliary/lp_solver/lp_solver_cplex.hxx>
#endif

#ifdef WITH_GUROBI
#include <opengm/inference/auxiliary/lp_solver/lp_solver_gurobi.hxx>
#endif

// export function
template<class GM, class ACC>
void export_lp_inference() {

   import_array();
   using namespace boost::python;
   std::string srName = semiRingName  <typename GM::OperatorType,ACC >() ;

   #ifdef WITH_CPLEX
   {
      append_subnamespace("solver");

      // setup
      InfSetup setup;
      setup.algType    = "linear-programming";
      setup.guarantees = "global optimal";
      setup.examples   = ">>> parameter = opengm.InfParam(integerConstraint=True)\n\n"
                         ">>> inference = opengm.inference.LpCplex2(gm=gm,accumulator='minimizer',parameter=parameter)\n\n"
                         "\n\n";
      setup.dependencies = "This algorithm needs the IBM CPLEX Optimizer, compile OpenGM with CMake-Flag ``WITH_CPLEX`` set to ``ON`` ";
      // export parameter
      typedef typename GM::ValueType ValueType;
      typedef typename GM::IndexType IndexType;

      typedef opengm::LpSolverCplex LpSolver;
      typedef opengm::LPInference<GM, opengm::Minimizer,LpSolver>    PyLPSolver;

      const std::string enumName1=std::string("_LpCplex2Relaxation")+srName;
      enum_<typename PyLPSolver::Relaxation> (enumName1.c_str())
         .value("firstOrder",  PyLPSolver::FirstOrder)
         .value("firstOrder2",   PyLPSolver::FirstOrder2)
      ;

      exportInfParam<PyLPSolver>("_LpCplex2");
      // export inference
      class_< PyLPSolver>("_LpCplex2",init<const GM & >())
      .def(InfSuite<PyLPSolver,false,true,false>(std::string("LpCplex2"),setup))
      .def(LpInferenceSuite<PyLPSolver>())
      ;

   }
   #endif

   #ifdef WITH_GUROBI
   {
      append_subnamespace("solver");

      // setup
      InfSetup setup;
      setup.algType    = "linear-programming";
      setup.guarantees = "global optimal";
      setup.examples   = ">>> parameter = opengm.InfParam(integerConstraint=True)\n\n"
                         ">>> inference = opengm.inference.LpGurobi(gm=gm,accumulator='minimizer',parameter=parameter)\n\n"
                         "\n\n";
      setup.dependencies = "This algorithm needs the Gurobi Optimizer, compile OpenGM with CMake-Flag ``WITH_GUROBI`` set to ``ON`` ";
      // export parameter
      typedef typename GM::ValueType ValueType;
      typedef typename GM::IndexType IndexType;

      typedef opengm::LpSolverGurobi LpSolver;
      typedef opengm::LPInference<GM, opengm::Minimizer,LpSolver>    PyLPSolver;

      const std::string enumName1=std::string("_LpGurobiRelaxation")+srName;
      enum_<typename PyLPSolver::Relaxation> (enumName1.c_str())
         .value("firstOrder",  PyLPSolver::FirstOrder)
         .value("firstOrder2",   PyLPSolver::FirstOrder2)
      ;

      exportInfParam<PyLPSolver>("_LpGurobi");
      // export inference
      class_< PyLPSolver>("_LpGurobi",init<const GM & >())
      .def(InfSuite<PyLPSolver,false,true,false>(std::string("LpGurobi"),setup))
      .def(LpInferenceSuite<PyLPSolver>())
      ;

   }
   #endif

}
// explicit template instantiation for the supported semi-rings
template void export_lp_inference<opengm::python::GmAdder, opengm::Minimizer>();


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticScatteringKernelFactor.cpp
//! \author Alex Robinson
//! \brief  Free gas elastic scattering gkq_set factor def.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <limits>

// Boost Includes
#include <boost/math/special_functions/bessel.hpp>

// FRENSIE Includes
#include "DataGen_FreeGasElasticScatteringKernelFactor.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "MonteCarlo_KinematicHelpers.hpp"
#include "Utility_ComparisonPolicy.hpp"
#include "Utility_DesignByContract.hpp"

namespace DataGen{

// Initialize static member data
double FreeGasElasticScatteringKernelFactor::neutron_kinetic_energy_multiplier=
  0.5*Utility::PhysicalConstants::neutron_rest_mass_energy/
  (Utility::PhysicalConstants::speed_of_light*
   Utility::PhysicalConstants::speed_of_light);

double FreeGasElasticScatteringKernelFactor::min_exp_arg =
  log(std::numeric_limits<double>::min());

// Constructor
FreeGasElasticScatteringKernelFactor::FreeGasElasticScatteringKernelFactor(
	  const std::shared_ptr<Utility::UnivariateDistribution>&
	  zero_temp_elastic_cross_section,
          const std::shared_ptr<MonteCarlo::NuclearScatteringAngularDistribution>&
	  cm_scattering_distribution,
	  const double A,
	  const double kT,
	  const double alpha,
	  const double beta,
	  const double E )
  : d_gkq_set( 1e-6 ),
    d_zero_temp_elastic_cross_section( zero_temp_elastic_cross_section ),
    d_cm_scattering_distribution( cm_scattering_distribution ),
    d_A( A ),
    d_kT( kT ),
    d_alpha( alpha ),
    d_beta( beta ),
    d_E( E )
{
  // Make sure the distributions are valid
  testPrecondition( zero_temp_elastic_cross_section.use_count() > 0 );
  testPrecondition( cm_scattering_distribution.use_count() > 0 );
  // Make sure the values are valid
  testPrecondition( A > 0.0 );
  testPrecondition( kT > 0.0 );
  testPrecondition( E > 0.0 );
  testPrecondition( beta >= MonteCarlo::calculateBetaMin( E, d_kT ) );
  remember( double alpha_min = MonteCarlo::calculateAlphaMin(E,beta,A,kT) );
  testPrecondition( alpha >= alpha_min );
  remember( double alpha_max = MonteCarlo::calculateAlphaMax(E,beta,d_A,d_kT) );
  testPrecondition( alpha <= alpha_max );

  calculateCachedValues();
}

// Set the alpha, beta, and energy values
void FreeGasElasticScatteringKernelFactor::setIndependentVariables(
							    const double alpha,
							    const double beta,
							    const double E )
{
  // Make sure the values are valid
  testPrecondition( E > 0.0 );
  testPrecondition( beta >= -E/d_kT );
  remember( double alpha_min_arg = sqrt(E)-sqrt(E+beta*d_kT) );
  remember( double alpha_min = alpha_min_arg*alpha_min_arg/(d_A*d_kT) );
  testPrecondition( alpha >= alpha_min );
  remember( double alpha_max_arg = sqrt(E)+sqrt(E+beta*d_kT) );
  remember( double alpha_max = alpha_max_arg*alpha_max_arg/(d_A*d_kT) );
  testPrecondition( alpha <= alpha_max );

  d_alpha = alpha;
  d_beta = beta;
  d_E = E;

  calculateCachedValues();
}

// Evaluate the factor at a desired value of the center-of-mass angle cosine
double FreeGasElasticScatteringKernelFactor::operator()(
						     const double mu_cm ) const
{
  // Make sure the cm angle is valid
  testPrecondition( mu_cm >= -1.0 );
  testPrecondition( mu_cm <= 1.0 );

  // The function goes to zero at mu_cm = 1.0
  if( mu_cm == 1.0 )
    return 0.0;
  else
  {
    double relative_velocity = d_relative_velocity_mult/sqrt(1.0-mu_cm);
    double relative_energy =
      neutron_kinetic_energy_multiplier*relative_velocity*
      relative_velocity;

    double term_1 =
      d_zero_temp_elastic_cross_section->evaluate( relative_energy )*
      d_cm_scattering_distribution->evaluatePDF( d_E, mu_cm )/
      ((1.0-mu_cm)*(1.0-mu_cm));

    double exp_arg = d_exponential_arg_mult/(1-mu_cm) +
      d_exponential_arg_const;

    double bessel_arg = d_bessel_arg_mult*
      sqrt((1.0+mu_cm)/(1.0-mu_cm));

    double term_2;

    try{
      if( exp_arg > min_exp_arg )
      {
	term_2 = boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	  exp( exp_arg );
      }
      else // Use extended precision
      {
	BoostLongDouble exp_arg_long = exp_arg;
	BoostLongDouble term_2_long =
	  boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	  exp( exp_arg_long );

	term_2 = term_2_long.convert_to<double>();
      }
    }
    // If the bessel argument is large use the asymptotic form of the function
    catch( std::exception& exception )
    {
      term_2 = exp( bessel_arg + exp_arg )/
	sqrt( 2*Utility::PhysicalConstants::pi*bessel_arg );
    }

    // Make sure the return value is valid
    testPostcondition(!Utility::QuantityTraits<double>::isnaninf(term_1*term_2));

    return term_1*term_2;
  }
}

// Get the integrated value
double FreeGasElasticScatteringKernelFactor::getIntegratedValue(
						 double& error_estimate ) const
{
  double integrated_value, lower_limit, upper_limit;

  this->findLimits( lower_limit, upper_limit );

  d_gkq_set.integrateAdaptively<15>( *this,
				    lower_limit,
				    upper_limit,
				    integrated_value,
				    error_estimate );

  // Make sure the value is valid
  testPostcondition( !Utility::QuantityTraits<double>::isnaninf( integrated_value ) );

  return integrated_value;
}

// Calculate the cached values
void FreeGasElasticScatteringKernelFactor::calculateCachedValues()
{
  d_energy_ratio = d_E/d_kT;

  d_exponential_arg_mult = -(d_A+1)*(d_A+1)*d_alpha/2.0;

  d_exponential_arg_const = -d_A*d_energy_ratio;

  double bessel_arg_mult_arg =
    4.0*d_A*d_alpha*d_energy_ratio -
    (d_beta - d_A*d_alpha)*(d_beta - d_A*d_alpha);

  // When alpha ~ alpha_min, alpha ~ alpha_max or beta ~ beta_min,
  // a very small negative argument is possible due to roundoff - set it to 0
  if( bessel_arg_mult_arg < 0.0 )
    bessel_arg_mult_arg = 0.0;

  d_bessel_arg_mult = (d_A+1)/2.0*sqrt(bessel_arg_mult_arg);

  d_relative_velocity_mult = (d_A+1)/d_A*
    sqrt(d_A*d_kT*d_alpha*Utility::PhysicalConstants::speed_of_light*
	 Utility::PhysicalConstants::speed_of_light/
	 Utility::PhysicalConstants::neutron_rest_mass_energy);
}

// Find limits to integrate over
void FreeGasElasticScatteringKernelFactor::findLimits(
						    double& lower_limit,
						    double& upper_limit ) const
{
  // find an independent value where the function is non-zero
  std::list<double> search_grid;

  double arg1 = 4*d_A*d_alpha*d_E/d_kT -
    (d_beta - d_A*d_alpha)*(d_beta - d_A*d_alpha);
  double arg2 = (d_A + 1)*(d_A + 1)*d_alpha*d_alpha;

  double estimated_peak_mu_cm = (arg1 - arg2)/(arg1 + arg2);

  double estimated_peak_exp_arg = d_exponential_arg_const +
    d_exponential_arg_mult/(1-estimated_peak_mu_cm) +
    d_bessel_arg_mult*
    sqrt((1.0+estimated_peak_mu_cm)/(1.0-estimated_peak_mu_cm));

  // Check if the integrand can be expected to return non-zero values
  if( estimated_peak_exp_arg > min_exp_arg )
  {
    search_grid.push_back( std::max( estimated_peak_mu_cm - 1e-6,
				     -1.0 ) );
    search_grid.push_back( std::min( estimated_peak_mu_cm + 1e-6,
				     1.0 ) );

    double center_value =
      this->findCMScatteringAngleCosineWithNonZeroFunctionValue( search_grid );

    // binary search to find closer lower and upper limits
    double tol = 1e-15;

    double lower_bound = -1.0;
    double upper_bound = center_value;
    double new_bound;

    while( Utility::RelativeErrorComparisonPolicy::calculateRelativeError(upper_bound, lower_bound ) > tol  )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( (*this)( new_bound ) == 0.0 )
	lower_bound = new_bound;
      else
	upper_bound = new_bound;
    }

    // set the lower integration limit
    lower_limit = lower_bound;

    lower_bound = center_value;
    upper_bound = 1.0;

    while( Utility::RelativeErrorComparisonPolicy::calculateRelativeError(upper_bound, lower_bound) > tol )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( (*this)( new_bound ) == 0.0 )
	upper_bound = new_bound;
      else
	lower_bound = new_bound;
    }

    // set the upper integration limit
    upper_limit = upper_bound;
  }
  else // integrand will always return zero
  {
    upper_limit = 0.0;
    lower_limit = 0.0;
  }
}

// Find a CM scattering angle cosine where the function is non-zero
double FreeGasElasticScatteringKernelFactor::findCMScatteringAngleCosineWithNonZeroFunctionValue(
					 std::list<double>& grid_points ) const
{
  std::list<double>::iterator first_grid_point = grid_points.begin();
  std::list<double>::iterator second_grid_point = first_grid_point;
  ++second_grid_point;

  double cm_center_value;

  // Find if any center points have non-zero function values
  while( second_grid_point != grid_points.end() )
  {
    cm_center_value = (*first_grid_point + *second_grid_point)/2;

    if( (*this)( cm_center_value ) > 0.0 )
      return cm_center_value;

    first_grid_point = grid_points.insert( second_grid_point,
					   cm_center_value );
    ++first_grid_point;
    ++second_grid_point;
  }

  return findCMScatteringAngleCosineWithNonZeroFunctionValue( grid_points );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticScatteringKernelFactor.cpp
//---------------------------------------------------------------------------//


/*
 * Copyright 2013-2017 Michael M. Magruder (https://github.com/mikemag)
 *
 * 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 <cmath>
#include <iostream>
#include <fstream>
#include <cassert>
#include <vector>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/program_options.hpp>

#include "machine.h"
#include "point.h"
#include "paths.h"
#include "shapes.h"
#include "gcode-writer.h"
#include "svg-writer.h"
#include "version.h"

using std::string;
using std::vector;

namespace MonkeyCAM {

// ***************************************************************
// * These are snippets that should be tests, but aren't yet. No *
// * guarantee they even build (this file isn't built.)          *
// ***************************************************************

void dumpPath(const Path& path) {
  int count = 0;
  fprintf(stderr, "---- path dump ----\n");
  for (const auto& p : path) {
    fprintf(stderr, "#%d (%lld, %lld)\n", count++,
            p.X.scaledInt(), p.Y.scaledInt());
  }
}

void misc(Machine& machine) {
  Point p1(4.2, 4.2, 4.2);
  fprintf(stderr, "MonkeyCAM stub: %lld\n", p1.X.scaledInt());

  EllipsePath ep { Point(5, 5), 2, 3, 0, 180 };

  ArcPath ap1 { Point(1, 1), Point(3, 0.8), Point(6, 1) };
  ArcPath ap2 { Point(6, 1), Point(3, 1.3), Point(1, 1) };
  ArcPath ap3 { Point(1, -1), Point(2, 0), Point(1, 1) };
  ArcPath ap4 { Point(1, 1), Point(0, 0), Point(1, -1) };
  ArcPath ap5 { Point(2, 1), Point(1, 2), Point(0, 1) };
  ArcPath ap6 { Point(0, 1), Point(1, 0), Point(2, 1) };
  ArcPath ap7 { Point(1, 2), Point(3, 2.5), Point(6, 2), ArcPath::Clockwise };
  ArcPath ap8 { Point(6, 2), Point(3, 1.5), Point(1, 2), ArcPath::Clockwise };
  Path cp;
  cp.push_back_path(ap1);
  cp.push_back_path(ap2);
  cp.push_back_path(ap3);
  cp.push_back_path(ap4);
  cp.push_back_path(ap5);
  cp.push_back_path(ap6);
  cp.push_back_path(ap7);
  cp.push_back_path(ap8);
  //dumpPath(cp);
  BezierPath bp { Point(1, 1), Point(1.5, 2), Point(2, 3), Point(3, 2) };
  //dumpPath(bp);
  SVGWriter svg("tmp.svg");
  svg.addPath(ap1);
  svg.addPath(ap2);
  svg.addPath(ap3);
  svg.addPath(ap4);
  svg.addPath(ap5);
  svg.addPath(ap6);
  svg.addPath(ap7);
  svg.addPath(ap8);
  svg.addPath(bp, "#ff0000");
  svg.addPath(ep);

  boost::property_tree::ptree boardConfig;
  read_json("b1.json", boardConfig);
  auto shape = loadBoard(boardConfig);
  auto profile = loadProfile(boardConfig, shape);

  auto overallPath = shape.buildOverallPath();
  auto basePath = shape.buildBasePath();

  SVGWriter svgBoard("shape-overview.svg", 16, 7);
  svgBoard.addPath(overallPath);
  svgBoard.addPath(basePath);
  svgBoard.addPath(profile.path());

  SVGWriter osvg("top-profile-overview.svg", 16, 7);
  osvg.addPath(overallPath, "#0000ff");
  std::vector<Path> resultPaths;
  int i = 0;
  do {
    resultPaths = PathUtils::OffsetPath(overallPath, -1.5 * 2.54 / 2 * ++i);
    for (auto const& p : resultPaths) osvg.addPath(p, "#00ff00");
  } while (!resultPaths.empty());

  auto tmpProgram = shape.generateTopProfile(machine, profile);

#if 0
  for (const auto& l : tmpProgram) {
    fprintf(stderr, "%s\n", l.c_str());
  }
#endif
  //dumpPath(top1);
}

} // namespace MonkeyCAM


#pragma once

// mapnik-vector-tile
#include "vector_tile_config.hpp"

// mapnik
#include <mapnik/box2d.hpp>

// mapbox
#include <mapbox/geometry/geometry.hpp>
#include <mapbox/geometry/wagyu/quick_clip.hpp>
#include <mapbox/geometry/wagyu/wagyu.hpp>

// boost
#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <iostream>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/unique.hpp>
#pragma GCC diagnostic pop

namespace mapnik
{

namespace vector_tile_impl
{

namespace detail
{

template <typename T>
double area(mapbox::geometry::linear_ring<T> const& poly) {
    std::size_t size = poly.size();
    if (size < 3) {
        return 0.0;
    }

    double a = 0.0;
    auto itr = poly.begin();
    auto itr_prev = poly.end();
    --itr_prev;
    a += static_cast<double>(itr_prev->x + itr->x) * static_cast<double>(itr_prev->y - itr->y);
    ++itr;
    itr_prev = poly.begin();
    for (; itr != poly.end(); ++itr, ++itr_prev) {
        a += static_cast<double>(itr_prev->x + itr->x) * static_cast<double>(itr_prev->y - itr->y);
    }
    return -a * 0.5;
}

inline mapbox::geometry::wagyu::fill_type get_wagyu_fill_type(polygon_fill_type type)
{
    switch (type)
    {
    case polygon_fill_type_max:
    case even_odd_fill:
        return mapbox::geometry::wagyu::fill_type_even_odd;
    case non_zero_fill:
        return mapbox::geometry::wagyu::fill_type_non_zero;
    case positive_fill:
        return mapbox::geometry::wagyu::fill_type_positive;
    case negative_fill:
        return mapbox::geometry::wagyu::fill_type_negative;
    }
    // Added return here to make gcc happy
    return mapbox::geometry::wagyu::fill_type_even_odd;
}

} // end ns detail

template <typename NextProcessor>
class geometry_clipper
{
private:
    NextProcessor & next_;
    mapnik::box2d<int> const& tile_clipping_extent_;
    double area_threshold_;
    bool strictly_simple_;
    bool multi_polygon_union_;
    polygon_fill_type fill_type_;
    bool process_all_rings_;
public:
    geometry_clipper(mapnik::box2d<int> const& tile_clipping_extent,
                     double area_threshold,
                     bool strictly_simple,
                     bool multi_polygon_union,
                     polygon_fill_type fill_type,
                     bool process_all_rings,
                     NextProcessor & next) :
              next_(next),
              tile_clipping_extent_(tile_clipping_extent),
              area_threshold_(area_threshold),
              strictly_simple_(strictly_simple),
              multi_polygon_union_(multi_polygon_union),
              fill_type_(fill_type),
              process_all_rings_(process_all_rings)
    {
    }

    void operator() (mapbox::geometry::point<std::int64_t> & geom)
    {
        next_(geom);
    }

    void operator() (mapbox::geometry::multi_point<std::int64_t> & geom)
    {
        // Here we remove repeated points from multi_point
        auto last = std::unique(geom.begin(), geom.end());
        geom.erase(last, geom.end());
        next_(geom);
    }

    void operator() (mapbox::geometry::geometry_collection<std::int64_t> & geom)
    {
        for (auto & g : geom)
        {
            mapbox::util::apply_visitor((*this), g);
        }
    }

    void operator() (mapbox::geometry::line_string<std::int64_t> & geom)
    {
        boost::geometry::unique(geom);
        if (geom.size() < 2)
        {
            return;
        }
        mapbox::geometry::multi_line_string<int64_t> result;
        mapbox::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        boost::geometry::intersection(clip_box, geom, result);
        if (result.empty())
        {
            return;
        }
        next_(result);
    }

    void operator() (mapbox::geometry::multi_line_string<std::int64_t> & geom)
    {
        if (geom.empty())
        {
            return;
        }

        mapbox::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        boost::geometry::unique(geom);
        mapbox::geometry::multi_line_string<int64_t> results;
        for (auto const& line : geom)
        {
            if (line.size() < 2)
            {
               continue;
            }
            boost::geometry::intersection(clip_box, line, results);
        }
        if (results.empty())
        {
            return;
        }
        next_(results);
    }

    void operator() (mapbox::geometry::polygon<std::int64_t> & geom)
    {
        if (geom.empty() || ((geom.front().size() < 3) && !process_all_rings_))
        {
            return;
        }

        mapbox::geometry::wagyu::wagyu<std::int64_t> clipper;
        mapbox::geometry::point<std::int64_t> min_pt(tile_clipping_extent_.minx(), tile_clipping_extent_.miny());
        mapbox::geometry::point<std::int64_t> max_pt(tile_clipping_extent_.maxx(), tile_clipping_extent_.maxy());
        mapbox::geometry::box<std::int64_t> b(min_pt, max_pt);

        bool first = true;
        for (auto & ring : geom) {
            if (ring.size() < 3)
            {
                if (first) {
                    if (process_all_rings_) {
                        first = false;
                    } else {
                        return;
                    }
                }
                continue;
            }
            double area = detail::area(ring);
            if (first) {
                first = false;
                if ((std::abs(area) < area_threshold_)  && !process_all_rings_) {
                    return;
                }
                if (area < 0) {
                    std::reverse(ring.begin(), ring.end());
                }
                auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                if (new_ring.empty()) {
                    if (process_all_rings_) {
                        continue;
                    }
                    return;
                }
                clipper.add_ring(new_ring);
            } else {
                if (std::abs(area) < area_threshold_) {
                    continue;
                }
                if (area > 0)
                {
                    std::reverse(ring.begin(), ring.end());
                }
                auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                if (new_ring.empty()) {
                    continue;
                }
                clipper.add_ring(new_ring);
            }
        }

        mapbox::geometry::multi_polygon<std::int64_t> mp;

        clipper.execute(mapbox::geometry::wagyu::clip_type_union,
                        mp,
                        detail::get_wagyu_fill_type(fill_type_),
                        mapbox::geometry::wagyu::fill_type_even_odd);

        if (mp.empty())
        {
            return;
        }
        next_(mp);
    }

    void operator() (mapbox::geometry::multi_polygon<std::int64_t> & geom)
    {
        if (geom.empty())
        {
            return;
        }

        mapbox::geometry::point<std::int64_t> min_pt(tile_clipping_extent_.minx(), tile_clipping_extent_.miny());
        mapbox::geometry::point<std::int64_t> max_pt(tile_clipping_extent_.maxx(), tile_clipping_extent_.maxy());
        mapbox::geometry::box<std::int64_t> b(min_pt, max_pt);
        mapbox::geometry::multi_polygon<std::int64_t> mp;

        if (multi_polygon_union_)
        {
            mapbox::geometry::wagyu::wagyu<std::int64_t> clipper;
            for (auto & poly : geom)
            {
                bool first = true;
                for (auto & ring : poly) {
                    if (ring.size() < 3)
                    {
                        if (first) {
                            first = false;
                            if (!process_all_rings_) {
                                break;
                            }
                        }
                        continue;
                    }
                    double area = detail::area(ring);
                    if (first) {
                        first = false;
                        if ((std::abs(area) < area_threshold_)  && !process_all_rings_) {
                            break;
                        }
                        if (area < 0) {
                            std::reverse(ring.begin(), ring.end());
                        }
                        auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                        if (new_ring.empty()) {
                            if (process_all_rings_) {
                                continue;
                            }
                            break;
                        }
                        clipper.add_ring(new_ring);
                    } else {
                        if (std::abs(area) < area_threshold_) {
                            continue;
                        }
                        if (area > 0)
                        {
                            std::reverse(ring.begin(), ring.end());
                        }
                        auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                        if (new_ring.empty()) {
                            continue;
                        }
                        clipper.add_ring(new_ring);
                    }
                }
            }
            clipper.execute(mapbox::geometry::wagyu::clip_type_union,
                            mp,
                            detail::get_wagyu_fill_type(fill_type_),
                            mapbox::geometry::wagyu::fill_type_even_odd);
        }
        else
        {
            for (auto & poly : geom)
            {
                mapbox::geometry::wagyu::wagyu<std::int64_t> clipper;
                mapbox::geometry::multi_polygon<std::int64_t> tmp_mp;
                bool first = true;
                for (auto & ring : poly) {
                    if (ring.size() < 3)
                    {
                        if (first) {
                            first = false;
                            if (!process_all_rings_) {
                                break;
                            }
                        }
                        continue;
                    }
                    double area = detail::area(ring);
                    if (first) {
                        first = false;
                        if ((std::abs(area) < area_threshold_)  && !process_all_rings_) {
                            break;
                        }
                        if (area < 0) {
                            std::reverse(ring.begin(), ring.end());
                        }
                        auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                        if (new_ring.empty()) {
                            if (process_all_rings_) {
                                continue;
                            }
                            break;
                        }
                        clipper.add_ring(new_ring);
                    } else {
                        if (std::abs(area) < area_threshold_) {
                            continue;
                        }
                        if (area > 0)
                        {
                            std::reverse(ring.begin(), ring.end());
                        }
                        auto new_ring = mapbox::geometry::wagyu::quick_clip::quick_lr_clip(ring, b);
                        if (new_ring.empty()) {
                            continue;
                        }
                        clipper.add_ring(new_ring);
                    }
                }
                clipper.execute(mapbox::geometry::wagyu::clip_type_union,
                                tmp_mp,
                                detail::get_wagyu_fill_type(fill_type_),
                                mapbox::geometry::wagyu::fill_type_even_odd);
                mp.insert(mp.end(), tmp_mp.begin(), tmp_mp.end());
            }
        }

        if (mp.empty())
        {
            return;
        }
        next_(mp);
    }
};

} // end ns vector_tile_impl
} // end ns mapnik


/*
*       FloatToFixPointGadget.cpp
*
*       Created on: March 10, 2014
*       Author: Hui Xue
*/

#include "FloatToFixPointGadget.h"
#include "mri_core_def.h"
#include "hoNDArray_math.h"

#include <boost/math/constants/constants.hpp>

namespace Gadgetron
{
    template<class T>
    constexpr auto ismrmrd_image_type(){
        if constexpr (std::is_same_v<T,unsigned short>) return ISMRMRD::ISMRMRD_USHORT;
        if constexpr (std::is_same_v<T,short>) return ISMRMRD::ISMRMRD_SHORT;
        if constexpr (std::is_same_v<T,int >) return ISMRMRD::ISMRMRD_INT;
        if constexpr (std::is_same_v<T,unsigned int>) return ISMRMRD::ISMRMRD_UINT;

        throw std::runtime_error("Unsupported type");

        }

    template<typename T, typename Base >
    void FloatToFixPointGadget<T,Base >::process(Core::InputChannel<Core::Image<float>> &input, Core::OutputChannel &output) {

        auto self = static_cast<Base&>(*this);

        auto clamp = [&](float val){
            return lround(std::min<float>(std::max<float>(val,self.min_intensity),self.max_intensity));
        };
        auto magnitude = [&](auto val){
            return T(clamp(std::abs(val)));
        };

        auto real_value = [&](auto val){
            return T(clamp(float(val)+self.intensity_offset));
        };

        auto phase = [&](float val){
            return T(clamp((val*self.intensity_offset/boost::math::float_constants::pi)+self.intensity_offset));
        };

        for (auto [img_header,data,meta] : input) {
            GDEBUG("Float to shorts norm: %f \n", nrm2(&data));
            auto output_data = hoNDArray<T>(data.dimensions());

            switch (img_header.image_type) {
                case ISMRMRD::ISMRMRD_IMTYPE_MAGNITUDE: {
                    std::transform(data.begin(),data.end(),output_data.begin(),magnitude);
                }
                    break;

                case ISMRMRD::ISMRMRD_IMTYPE_REAL:
                case ISMRMRD::ISMRMRD_IMTYPE_IMAG: {
                    std::transform(data.begin(),data.end(),output_data.begin(),real_value);

                    if (meta) {
                        if (meta->length(GADGETRON_IMAGE_WINDOWCENTER) > 0) {
                            long windowCenter;
                            windowCenter = meta->as_long(GADGETRON_IMAGE_WINDOWCENTER, 0);
                            meta->set(GADGETRON_IMAGE_WINDOWCENTER,
                                                    windowCenter + (long) self.intensity_offset);
                        }
                    }
                }
                    break;

                case ISMRMRD::ISMRMRD_IMTYPE_PHASE: {
                    std::transform(data.begin(),data.end(),output_data.begin(),phase );

                }
                    break;

                default:
                    throw std::runtime_error("Unknown image type in Image");

            }

            img_header.data_type = ismrmrd_image_type<T>();
            output.push(img_header,std::move(output_data),std::move(meta));
        }

    }

    GADGETRON_GADGET_EXPORT(FloatToUShortGadget)
    GADGETRON_GADGET_EXPORT(FloatToShortGadget)
    GADGETRON_GADGET_EXPORT(FloatToIntGadget)
    GADGETRON_GADGET_EXPORT(FloatToUIntGadget)
}


/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2013, The University of Texas at Dallas
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * 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 The University of Texas at Dallas 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.
 *
 */

/*
 * Filename: connectivity_control_node.cpp
 *
 * Description: This file contains the ROS node that calculates the velocity commands
 * for an individual iRobot Create.
 *
 * Log
 * ----
 * 2014-02-21 File created by Hazen Eckert
 *
 */

// ROS includes
#include "ros/ros.h"
#include "ros/assert.h"
#include "dynamic_reconfigure/server.h"
#include "create_driver/vicon_driver.h"
#include "geometry_msgs/Twist.h"

// Library includes
#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/LU>

// Local Package includes
#include "connectivity_controller/ControlVariablesConfig.h"
#include "connectivity_controller/ConnectivityMsgs.h"
#include "connectivity_controller/ControlMsgs.h"

using namespace std;
using namespace Eigen;

// Constants
const double PI = 3.1415926535;
const double alphaOverBar = 5;
const double alphaUnderBar = 0;

// Connectivity variables
vector<double> trace_Mx;
vector<double> trace_My;

vector<double> control_input_CA_x;
vector<double> control_input_CA_y;

vector<double> control_input_FC_x;
vector<double> control_input_FC_y;

double detM;

// Dynamic reconfigure variables
double k = 0.5; // linear velocity gain
double ktheta = 5.0; // angular velocity gain
bool power = true;
double x_bar_desired = 0.25;
double y_bar_desired = 0.5;
double psi_desired = 3*PI/4;
double k_bar = 1;
double kconn = 1.0e-01,kca = 2.0e-02,kfc = 1.0;
bool formation = true;

// Callbacks
void ConnectCallback(const connectivity_controller::ConnectivityMsgs::ConstPtr& msg) {
	trace_Mx.assign(msg->trMx.begin(), msg->trMx.end());
	trace_My.assign(msg->trMy.begin(), msg->trMy.end());

	control_input_CA_x.assign(msg->u_ca_x.begin(), msg->u_ca_x.end());
	control_input_CA_y.assign(msg->u_ca_y.begin(), msg->u_ca_y.end());

	control_input_FC_x.assign(msg->u_fc_x.begin(), msg->u_fc_x.end());
	control_input_FC_y.assign(msg->u_fc_y.begin(), msg->u_fc_y.end());

	detM = msg->detM;
}

void reconfigureCallback(connectivity_controller::ControlVariablesConfig &config, uint32_t level) {
	k = config.k;
	ktheta = config.ktheta;
	power = config.power;
	x_bar_desired = config.x_bar_desired;
	y_bar_desired = config.y_bar_desired;
	psi_desired = config.psi_desired;
	k_bar = config.k_bar;
	kconn = config.kconn;
	kca = config.kca;
	kfc = config.kfc;
	formation = config.formation;
}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "control");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	// ROS Parameters

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// Robot name
	string robot_name;
	private_n.param<std::string>("robotname", robot_name, "defaultname");

	int index = find(robot_names.begin(), robot_names.end(), robot_name) - robot_names.begin();

	// ROS Dynamic Reconfigure

	dynamic_reconfigure::Server<connectivity_controller::ControlVariablesConfig> server;
  	dynamic_reconfigure::Server<connectivity_controller::ControlVariablesConfig>::CallbackType f;
  	f = boost::bind(&reconfigureCallback, _1, _2);
 	server.setCallback(f);

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;
	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe("/" + *name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	sub.push_back(n.subscribe( "/connectivity", 10, ConnectCallback));

	// ROS Publishers
	ros::Publisher vel = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);

	ros::Publisher dia = n.advertise<connectivity_controller::ControlMsgs>("dia", 10);

	// ROS loop
	ros::Rate loop_rate(250); // 250 Hz

	while (ros::ok())
	{
		// Retrieve Vicon Data
		ros::spinOnce();

		if(trace_Mx.size() < num_robots) {
			ROS_WARN("Connectivity is not publishing large enough vectors: %d < numrobots", trace_Mx.size());
			continue;
		}

		MatrixXd S(2*num_robots, 3);

		for (int i = 0; i < num_robots; i++)
		{
			Vector3d row;
			double x = -(num_robots - 1)*vicon[robot_names[i]].y();

			for (int j = 0; j < num_robots; j++)
			{
				if (j != i) {
					x += vicon[robot_names[j]].y();
				}
			}

			row << 1, 0, x/num_robots;

			S.row(2*i) = row;

			x = (num_robots - 1)*vicon[robot_names[i]].x();

			for (int j = 0; j < num_robots; j++)
			{
				if (j != i) {
					x -= vicon[robot_names[j]].x();
				}

			}

			row << 0, 1, x/num_robots;

			S.row(2*i + 1) = row;
		}

		double x_bar = 0.0;
		for (int i = 0; i < num_robots; i++)
		{
			x_bar += vicon[robot_names[i]].x();
		}
		x_bar /= ((double)num_robots);

		double y_bar = 0.0;
		for (int i = 0; i < num_robots; i++)
		{
			y_bar += vicon[robot_names[i]].y();
		}
		y_bar /= ((double)num_robots);

		double psi = atan2(vicon[robot_names[num_robots/2 -1]].y() - vicon[robot_names[0]].y(),
							vicon[robot_names[num_robots/2 -1]].x() - vicon[robot_names[0]].x());

		if (psi > PI){
			psi = psi - 2*PI;
		} else if (psi < -PI){
			psi = psi + 2*PI;
		}

		MatrixXd eta(3,1);
		eta << 	(x_bar - x_bar_desired),
				(y_bar - y_bar_desired),
				(psi - psi_desired);

		double uconnx = (((pow(alphaOverBar,2)-pow(alphaUnderBar,2))
				     *(pow(detM, 2)-pow(alphaOverBar,2)))
				     /(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)))
				     *pow(detM,2)*trace_Mx[index];

		double ufx = control_input_FC_x[index];	// Formation control
		double ucx = control_input_CA_x[index];		// Collision avoidance

		double uconny = (((pow(alphaOverBar,2)-pow(alphaUnderBar,2))
				     *(pow(detM, 2)-pow(alphaOverBar,2)))
				     /(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)))
				     *pow(detM,2)*trace_My[index];

		double ufy = control_input_FC_y[index];	// Formation control
		double ucy = control_input_CA_y[index];		// Collision avoidance

		if ( detM < alphaUnderBar )
		{
			uconnx = 0.0;
			uconny = 0.0;
		}
		else if (detM > alphaOverBar )
		{
			uconnx = 0.0;
			uconny = 0.0;
		}

		MatrixXd u_bar = S*eta;

		double ux = kconn*uconnx + kfc*ufx + kca*ucx;
		if ( !formation ) {
			ux = kconn*uconnx - k_bar*u_bar(2*index,0) + kca*ucx;
		}

		double uy = kconn*uconny + kfc*ufy + kca*ucy;
		if ( !formation ) {
			uy = kconn*uconny - k_bar*u_bar(2*index + 1,0) + kca*ucy;
		}

		double thetad = atan2(uy, ux);
		if ( !formation ) {
			thetad = atan2(uconny - u_bar(2*index+1,0) + ucy, uconnx - u_bar(2*index,0) + ucx);
		}

		double etheta = vicon[robot_name].theta() - thetad;

		double v = k*1000*cos(etheta)*sqrt(pow(ux,2) + pow(uy,2));;
		double w = -ktheta*etheta;

		// std::cerr << "v = " << v << std::endl;
		// std::cerr << "w = " << w << std::endl;
		// std::cerr << "ux = " << ux << std::endl;
		// std::cerr << "uconnx = " << uconnx << std::endl;
		// std::cerr << "ufx = " << ufx << std::endl;
		// std::cerr << "ucx = " << ucx << std::endl;
		// std::cerr << "(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(detM, 2)-pow(alphaOverBar,2)))/(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)))  = " << (((pow(alphaOverBar,2)-pow(alphaUnderBar,2))
		// 		     *(pow(detM, 2)-pow(alphaOverBar,2)))
		// 		     /(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)))  << std::endl;
		// std::cerr << "pow(detM,2)*trace_Mx[index] = " << pow(detM,2)*trace_Mx[index] << std::endl;
		// std::cerr << "((pow(alphaOverBar,2)-pow(alphaUnderBar,2)) *(pow(detM, 2)-pow(alphaOverBar,2))) = " << ((pow(alphaOverBar,2)-pow(alphaUnderBar,2))
		// 		     *(pow(detM, 2)-pow(alphaOverBar,2))) << std::endl;
		// std::cerr << "1 /(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)) = " << 1 /(pow(pow(detM, 2)-pow(alphaUnderBar,2),3)) << std::endl;

		if ( power ) {
		// Send wheel velocities to driver
			geometry_msgs::Twist msg;
			msg.linear.x = v;
			msg.linear.y = 0;
			msg.linear.z = 0;
			msg.angular.x = 0;
			msg.angular.y = 0;
			msg.angular.z = w;
			vel.publish(msg);
		} else {
			geometry_msgs::Twist msg;
			msg.linear.x = 0;
			msg.linear.y = 0;
			msg.linear.z = 0;
			msg.angular.x = 0;
			msg.angular.y = 0;
			msg.angular.z = 0;
			vel.publish(msg);
		}

		connectivity_controller::ControlMsgs dmsg;
		dmsg.ux = ux;
		dmsg.uy = uy;
		dmsg.etheta = etheta;
		dmsg.thetad = thetad;
		dmsg.uconny = kconn*uconny;
		dmsg.ufy = kfc*ufy;
		dmsg.ucy = kca*ucy;
		dmsg.uconny = kconn*uconnx;
		dmsg.ufy = kfc*ufx;
		dmsg.ucy = kca*ucx;
		dmsg.psi = psi;

		dia.publish(dmsg);

		loop_rate.sleep();
	}

	return 0;
}