/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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 "XMLRWParser.hpp"

using namespace rw::math;

#include "MultipleFileIterator.hpp"
#include "XMLErrorHandler.hpp"
#include "XMLParser.hpp"
#include "XMLRWPreParser.hpp"

#include <rw/core/macros.hpp>
#include <rw/math/Constants.hpp>

#include <boost/spirit/include/classic_parse_tree.hpp>

#include <rw/math/Transform3D.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/RPY.hpp>
#include <memory>

using namespace phoenix;
using namespace boost::spirit::classic;
using namespace boost::spirit;
using namespace rw::loaders;

namespace {

struct FilePosParser
{
    typedef file_position result_t;
    template< typename ScannerT >
    std::ptrdiff_t operator() (ScannerT const& scan, result_t& result) const
    {
        result = scan.first.get_position ();
        return 0;
    }
};

functor_parser< FilePosParser > filepos_p;

struct Transform3DParser
    : public grammar< Transform3DParser, result_closure< Transform3D<> >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return transform_r; }

        definition (Transform3DParser const& self)
        {
            transform_r = (matrix_transform_r | rpypos_transform_r);

            rpy_r =
                XMLElem_p ("RPY",
                           real_p[var (_rpy (0)) = arg1 * Deg2Rad] >>
                               real_p[var (_rpy (1)) = arg1 * Deg2Rad] >>
                               real_p[var (_rpy (2)) = arg1 * Deg2Rad][rpy_r.result_ = var (_rpy)]);

            pos_r = XMLElem_p ("Pos",
                               real_p[var (_pos (0)) = arg1] >> real_p[var (_pos (1)) = arg1] >>
                                   real_p[var (_pos (2)) = arg1][pos_r.result_ = var (_pos)]);

            rpypos_transform_r =
                ((rpy_r[var (_rpy) = arg1] >>
                  pos_r[var (_pos) = arg1])[self.result_ = construct_< Transform3D<> > (
                                                var (_pos), var (_rpy))]) |
                ((pos_r[var (_pos) = arg1] >>
                  rpy_r[var (_rpy) = arg1])[self.result_ = construct_< Transform3D<> > (
                                                var (_pos), var (_rpy))])

                ;

            auto func_s = [&] (double d) {_t3d_v.clear(); _t3d_v.push_back(d); };
            auto func = [&] (double d) {_t3d_v.push_back(d);};
            auto func_e = [&] (double d) {_t3d_v.push_back(d); for( size_t i = 0; i < _t3d_v.size(); i++) {_t3d(i/4,i%4) = _t3d_v[i]; } };
            matrix_transform_r = XMLElem_p (
                "Transform",
                    real_p[func_s] >> real_p[func] >>
                    real_p[func] >> real_p[func] >>
                    real_p[func] >> real_p[func] >>
                    real_p[func] >> real_p[func] >>
                    real_p[func] >> real_p[func] >>
                    real_p[func] >>
                    real_p[func_e][self.result_ = var (_t3d)]);
        }

      private:
        rule< ScannerT > transform_r, matrix_transform_r, rpypos_transform_r;
        rule< ScannerT, result_closure< RPY< double > >::context_t > rpy_r;
        rule< ScannerT, result_closure< Vector3D< double > >::context_t > pos_r;
        // temp variables
        Transform3D< double > _t3d;
        std::vector<double> _t3d_v;
        RPY< double > _rpy;
        Vector3D< double > _pos;
    };
} transform3d_p;

struct QuotedStringParser
    : public grammar< QuotedStringParser, result_closure< std::string >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return quotedStr_p; }

        definition (QuotedStringParser const& self)
        {
            quotedStr_p =
                token_node_d[ch_p ('"') >>
                             lexeme_d[*(anychar_p - '"')]
                                     [self.result_ = construct_< std::string > (arg1, arg2)] >>
                             ch_p ('"')];
        }

      private:
        rule< ScannerT > quotedStr_p;
    };
} quotedStr_p;

struct XMLAttrStringParser
    : public grammar< XMLAttrStringParser, result_closure< std::string >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return attrstr_r; }

        definition (XMLAttrStringParser const& self)
        {
            attrstr_r = (*(anychar_p - '"'))[self.result_ = construct_< std::string > (arg1, arg2)];
        }

      private:
        rule< ScannerT > attrstr_r;
    };
} attrstr_p;

struct PropertyParser : public grammar< PropertyParser, result_closure< DummyProperty >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return property_r; }

        definition (PropertyParser const& self)
        {
            property_r = XMLAttElem_p (
                "Property",
                XMLAtt_p ("name", attrstr_p[var (_property._name) = arg1]) >>
                    !(XMLAtt_p ("type", attrstr_p[var (_property._type) = arg1])) >>
                    !(XMLAtt_p ("refframe", attrstr_p[var (_property._refframe) = arg1])) >>
                    !(XMLAtt_p ("desc", attrstr_p[var (_property._desc) = arg1])),
                (*(anychar_p -
                   (str_p ("</") >>
                    "Property")))[var (_property._val) = construct_< std::string > (arg1, arg2)]
                                 [self.result_ = var (_property)]);
        }

      private:
        rule< ScannerT > property_r;
        DummyProperty _property;
    };
} property_p;

struct ModelParser : public grammar< ModelParser, result_closure< DummyModel >::context_t >
{
  public:
    std::vector< std::string >& _scope;

    ModelParser (std::vector< std::string >& scope) : _scope (scope) {}

    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return model_r; }

        definition (ModelParser const& self)
        {
            model_r =
                eps_p[var (_model) = construct_< DummyModel > ()] >>
                (drawable_r |
                 colmodel_r)[var (_model._scope) = var (self._scope)][self.result_ = var (_model)];

            colmodel_r =    // eps_p[ var( _model ) = colmodel_r.result_ ] >> // used to initialize
                            // _model
                (XMLAttElem_p (
                    "CollisionModel",
                    (XMLAtt_p ("name", attrstr_p[var (_model._name) = arg1])) >>

                        !(XMLAtt_p ("refframe", attrstr_p[var (_model._refframe) = arg1])),
                    !transform3d_p[var (_model._transform) = arg1] >>

                        *(geometry_r[push_back_a (_model._geo)])))[var (_model._isDrawable) =
                                                                       false];

            drawable_r = (XMLAttElem_p (
                "Drawable",
                (XMLAtt_p ("name", attrstr_p[var (_model._name) = arg1])) >>
                    !(XMLAtt_p ("refframe", attrstr_p[var (_model._refframe) = arg1])) >>
                    !(XMLAtt_p ("colmodel",
                                (str_p ("Enabled")[var (_model._colmodel) = true] |
                                 str_p ("Disabled")[var (_model._colmodel) =
                                                        false]))),    // used to initialize _model
                !transform3d_p[var (_model._transform) = arg1] >> *(rgb_r) >>
                    *(geometry_r[push_back_a (_model._geo)])))[var (_model._isDrawable) = true];

            rgb_r = XMLElem_p (
                "RGB",
                real_p[var (_model._r) = arg1] >> real_p[var (_model._g) = arg1] >>
                    real_p[var (_model._b) = arg1] >>
                    *real_p[var (_model._a) = arg1][var (_model._customMaterial) = true]);

            geometry_r =
                eps_p[var (_geo) = construct_< DummyGeometry > ()] >>
                (XMLAttElem_p (
                     "Polytope",
                     XMLAtt_p (
                         "file",
                         attrstr_p[var (_geo._filename) = arg1][var (_geo._type) = PolyType]) >>
                         filepos_p[var (_geo._pos) = arg1],
                     eps_p)    // save position of file

                 | XMLAttElem_p ("Plane", eps_p[var (_geo._type) = PlaneType], eps_p) |
                 XMLAttElem_p (
                     "Sphere",
                     XMLAtt_p ("radius",
                               real_p[var (_geo._radius) = arg1][var (_geo._type) = SphereType]) >>
                         *XMLAtt_p ("level", real_p[var (_geo._level) = arg1]),
                     eps_p) |
                 XMLAttElem_p (
                     "Box",
                     XMLAtt_p ("x", real_p[var (_geo._x) = arg1]) >>
                         XMLAtt_p ("y", real_p[var (_geo._y) = arg1]) >>
                         XMLAtt_p ("z", real_p[var (_geo._z) = arg1][var (_geo._type) = CubeType]),
                     eps_p) |
                 XMLAttElem_p (
                     "Cone",
                     XMLAtt_p ("radius", real_p[var (_geo._radius) = arg1]) >>
                         XMLAtt_p ("z",
                                   real_p[var (_geo._z) = arg1][var (_geo._type) = ConeType]) >>
                         *XMLAtt_p ("level", real_p[var (_geo._level) = arg1]),
                     eps_p) |
                 XMLAttElem_p (
                     "Cylinder",
                     XMLAtt_p ("radius", real_p[var (_geo._radius) = arg1]) >>
                         XMLAtt_p ("z",
                                   real_p[var (_geo._z) = arg1][var (_geo._type) = CylinderType]) >>
                         *XMLAtt_p ("level", real_p[var (_geo._level) = arg1]),
                     eps_p) |
                 XMLAttElem_p (
                     "Tube",
                     XMLAtt_p ("radius", real_p[var (_geo._radius) = arg1]) >>
                         XMLAtt_p ("thickness", real_p[var (_geo._x) = arg1]) >>
                         XMLAtt_p ("z",
                                   real_p[var (_geo._z) = arg1][var (_geo._type) = TubeType]) >>
                         *XMLAtt_p ("level", real_p[var (_geo._level) = arg1]),
                     eps_p) |
                 XMLAttElem_p (
                     "Custom",
                     XMLAtt_p (
                         "type",
                         attrstr_p[var (_geo._filename) = arg1][var (_geo._type) = CustomType]) >>
                         XMLAtt_p ("param", attrstr_p[var (_geo._parameters) = arg1]),
                     eps_p))[geometry_r.result_ = var (_geo)];
        }

      private:
        rule< ScannerT > model_r, colmodel_r, drawable_r;
        rule< ScannerT > rgb_r;
        rule< ScannerT, result_closure< DummyGeometry >::context_t > geometry_r;
        DummyModel _model;
        DummyGeometry _geo;
    };
};

struct RigidBodyParser
    : public grammar< RigidBodyParser, result_closure< DummyRigidBody >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return rigidbody_r; }

        definition (RigidBodyParser const& self)
        {
            rigidbody_r =
                eps_p[var (_rigid) = construct_< DummyRigidBody > ()] >>
                XMLAttElem_p ("RigidBody",
                              !(XMLAtt_p ("refframe", attrstr_p[var (_rigid._refframe) = arg1])),
                              *(transform3d_p[var (_rigid._transform) = arg1] |
                                XMLElem_p ("Mass", real_p[var (_rigid._mass) = arg1]) |
                                inertiamatrix_r))[self.result_ = var (_rigid)];

            inertiamatrix_r = XMLElem_p ("InertiaMatrix",
                                         real_p >> real_p >> real_p >> real_p >> real_p >> real_p >>
                                             real_p >> real_p >> real_p) |
                              (XMLElem_p ("LambdaX", real_p) >> XMLElem_p ("LambdaY", real_p) >>
                               XMLElem_p ("LambdaZ", real_p));
        }

      private:
        rule< ScannerT > rigidbody_r, inertiamatrix_r;
        DummyRigidBody _rigid;
        ModelParser model_p;
    };
};

struct FrameParser : public grammar< FrameParser, result_closure< DummyFrame >::context_t >
{
  public:
    std::vector< std::string >& _scope;

    FrameParser (std::vector< std::string >& scope) : _scope (scope) {}

    template< typename ScannerT > struct definition
    {
      public:
        rule< ScannerT > const start () const { return frame_r; }

        definition (FrameParser const& self) : model_p (self._scope)
        {
            frame_r =
                eps_p[var (_frame) = construct_< DummyFrame > ()] >>
                XMLAttElem_p (
                    "Frame",
                    XMLAtt_p ("name", attrstr_p[var (_frame._name) = arg1]) >>
                        !(XMLAtt_p ("refframe", attrstr_p[var (_frame._refframe) = arg1])) >>
                        !(XMLAtt_p ("type", attrstr_p[var (_frame._type) = arg1])) >>
                        !(XMLAtt_p ("daf",
                                    (str_p ("true")[var (_frame._isDaf) = true] |
                                     str_p ("false")[var (_frame._isDaf) = false]))),
                    // parse transform
                    !(transform3d_p[var (_frame._transform) = arg1]) >>
                        // parse properties
                        *(property_p[push_back_a (_frame._properties)] |
                          model_p[push_back_a (_frame._models)]))[self.result_ = var (_frame)];
        }

      private:
        rule< ScannerT > frame_r;
        DummyFrame _frame;
        ModelParser model_p;
    };
};

struct XMLDeviceParser : public grammar< XMLDeviceParser, result_closure< DummyDevice >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      private:
        std::vector< std::string > _scope;
        std::string _currentDir;
        std::string _currentFile;
        DummyProperty _property;
        std::string _refjoint;
        DummyDevice _dev;
        DummyFrame _frame;
        DummyLimit _limit;
        DummyGeometry _geo;
        DummyModel _model;
        DHParam _dhparam;
        DummyCollisionSetup _setup;
        DummyCalibration _calibration;
        DummyProximitySetup _psetup;
        QConfig _config;
        QJunction _junction;
        const QConfig emptyConfig;
        const QJunction emptyJunction;
        Transform3DParser t3d_p;
        rule< ScannerT > device_r, serialdevice_r, paralleldevice_r, treedevice_r, joint_r,
            dhjoint_r, depend_r, devicebody_r, chainbody_r, serialchain_r, jointstate_r,
            mobiledevice_r, configuration_r, junction_r;

        // rule<ScannerT, result_closure<DummyConveyorSegment>::context_t> conveyorsegment_r

        rule< ScannerT, result_closure< DummyLimit >::context_t > jointlimit_r;

        rule< ScannerT, result_closure< DummyCollisionSetup >::context_t > colsetup_r;
        rule< ScannerT, result_closure< DummyProximitySetup >::context_t > proxsetup_r;

        ModelParser model_p;
        FrameParser frame_p;

      public:
        /**
         * @brief Gets start rule
         * @return the start rule
         */
        rule< ScannerT > const start () const { return device_r; }

        /**
         * @brief Gets parser definition
         */
        definition (XMLDeviceParser const& self) : model_p (_scope), frame_p (_scope)
        {
            device_r = eps_p[var (_dev._scope) = _scope] >>
                       (serialdevice_r | paralleldevice_r | treedevice_r |
                        mobiledevice_r)[self.result_ = var (_dev)];

            serialchain_r = XMLAttElem_p ("SerialChain",
                                          XMLAtt_p ("name", attrstr_p[EnterScope (_scope)]),
                                          chainbody_r)[LeaveScope (_scope)];

            chainbody_r =
                *(frame_p[AddFrameToDevice (_frame, _dev, _scope)] |
                  joint_r[AddFrameToDevice (_frame, _dev, _scope)] |
                  dhjoint_r[AddFrameToDevice (_frame, _dev, _scope)] |
                  model_p[InsertModelInMap (_model, _model._refframe, _scope, _dev._modelMap)] |
                  jointlimit_r[InsertLimitInMap (_scope, _dev._limitMap)] |
                  property_p[InsertPropertyInMap (_scope, _dev._propertyMap)] | serialchain_r |
                  colsetup_r[push_back_a (_dev._colsetups)] |
                  proxsetup_r[push_back_a (_dev._proxsetups)]);

            colsetup_r = XMLAttElem_p (
                "CollisionSetup",
                XMLAtt_p (
                    "file",
                    attrstr_p[var (_setup._filename) = arg1][var (_setup._scope) = var (_scope)] >>
                        filepos_p[var (_setup._pos) = arg1]),
                eps_p)[colsetup_r.result_ = var (_setup)];

            /*calibration_r =
                            XMLAttElem_p("Calibration",
                                            XMLAtt_p("file", attrstr_p[ var(_calibration._filename)
               = arg1 ] [ var( _calibration._scope ) = var( _scope ) ] >> filepos_p[
               var(_calibration._pos) = arg1 ]
                              ),
                              eps_p
                          )[ calibration_r.result_ = var(_calibration) ];

                            */
            proxsetup_r = XMLAttElem_p ("ProximitySetup",
                                        XMLAtt_p ("file",
                                                  attrstr_p[var (_psetup._filename) = arg1]
                                                           [var (_psetup._scope) = var (_scope)] >>
                                                      filepos_p[var (_psetup._pos) = arg1]),
                                        eps_p)[proxsetup_r.result_ = var (_psetup)];

            devicebody_r = chainbody_r >>
                           //! frames >> // frames of interest
                           (*configuration_r[AddConfigToDevice (_config, _dev)]
                                            [var (_config) = var (emptyConfig)])

                ;

            configuration_r = XMLAttElem_p ("Q",
                                            XMLAtt_p ("name", attrstr_p[var (_config.name) = arg1]),
                                            *real_p[push_back_a (_config.q)]);

            junction_r = XMLElem_p (
                "Junction",
                *XMLElem_p (
                    "Chains",
                    (*(anychar_p - (str_p ("</") >> "Chains")))[push_back_a (_junction.chains)]

                    ));

            treedevice_r =
                eps_p[var (_dev) = construct_< DummyDevice > ()] >>
                XMLAttElem_p (
                    "TreeDevice",
                    XMLAtt_p ("name",
                              attrstr_p[var (_dev._name) = arg1][var (_dev._type) = TreeType]
                                       [EnterScope (_scope)]),
                    devicebody_r)[LeaveScope (_scope)];

            serialdevice_r =
                eps_p[var (_dev) = construct_< DummyDevice > ()] >>
                XMLAttElem_p (
                    "SerialDevice",
                    XMLAtt_p ("name",
                              attrstr_p[var (_dev._name) = arg1][var (_dev._type) = SerialType]
                                       [EnterScope (_scope)]),
                    devicebody_r)[LeaveScope (_scope)];

            paralleldevice_r =
                eps_p[var (_dev) = construct_< DummyDevice > ()] >>
                XMLAttElem_p (
                    "ParallelDevice",
                    XMLAtt_p ("name",
                              attrstr_p[var (_dev._name) = arg1][var (_dev._type) = ParallelType]
                                       [EnterScope (_scope)]),
                    chainbody_r >> (*junction_r[AddJunctionToDevice (_junction, _dev)]
                                               [var (_junction) = var (emptyJunction)]) >>
                        (*configuration_r[AddConfigToDevice (_config, _dev)]
                                         [var (_config) = var (emptyConfig)]))[LeaveScope (_scope)];

            mobiledevice_r =
                eps_p[var (_dev) = construct_< DummyDevice > ()] >>
                XMLAttElem_p (
                    "MobileDevice",
                    XMLAtt_p ("name",
                              attrstr_p[var (_dev._name) = arg1][var (_dev._type) = MobileType]
                                       [EnterScope (_scope)]) >>
                        XMLAtt_p ("basename", attrstr_p[var (_dev._basename) = arg1]),
                    XMLElem_p ("AxelWidth", real_p[var (_dev._axelwidth) = arg1]) >>
                        XMLAttElem_p ("LeftWheel",
                                      XMLAtt_p ("name", attrstr_p[var (_dev._leftname) = arg1]),
                                      eps_p) >>
                        XMLAttElem_p ("RightWheel",
                                      XMLAtt_p ("name", attrstr_p[var (_dev._rightname) = arg1]),
                                      eps_p) >>
                        devicebody_r)[LeaveScope (_scope)];

            /*
                            conveyordevice_r = eps_p[ var( _dev ) = construct_<DummyDevice>() ] >>
                                XMLAttElem_p("ConveyorDevice",
                                    XMLAtt_p("name", attrstr_p
                                        [ var( _dev._name ) = arg1 ]
                                        [ var( _dev._type ) = ConveyorType ]
                                        [ EnterScope(_scope) ]),
                                    *conveyorsegment_r
                                        [ push_back_a( _dev._conveyorsegments ) ] >>
                                    devicebody_r
                                )[ LeaveScope(_scope) ];

                            conveyorsegment_r =
                                XMLAttElem_p("ConveyorSegment",
                                    XMLAtt_p("type", attrstr_p
                                        [ var( _dev._name ) = arg1 ]
                                        [ var( _dev._type ) = ConveyorType ]
                                        [ EnterScope(_scope) ]),
                                    *conveyorsegment_r
                                        [ push_back_a( _dev._conveyorsegments ) ] >>
                                    devicebody_r
                                );
                                ;
              */
            jointstate_r = (str_p ("Active")[var (_frame._state) = ActiveState] |
                            str_p ("Passive")[var (_frame._state) = PassiveState]);

            joint_r = eps_p[var (_frame) = construct_< DummyFrame > ()] >>    // init _frame
                      XMLAttElem_p (
                          "Joint",
                          XMLAtt_p ("name", attrstr_p[var (_frame._name) = arg1]) >>
                              !(XMLAtt_p ("refframe", attrstr_p[var (_frame._refframe) = arg1])) >>
                              XMLAtt_p ("type", attrstr_p[var (_frame._type) = arg1]) >>
                              !(XMLAtt_p ("state", jointstate_r)),
                              !(t3d_p[var (_frame._transform) = arg1]) >>
                              *(jointlimit_r[push_back_a (_frame._limits)] |
                                depend_r |
                                property_p[push_back_a (_frame._properties)] |
                                model_p[push_back_a (_frame._models)]));
            // T alpha, T a,   beta, b,
            // T alpha, T a,   d, theta
            /*

+   if (isParallel)
+       return Transform3D(
+               Vector3D<T>(a * cos(beta), b, - a * sin(beta)),
+               Rotation3D<T>(
+                   cos(beta), sin(alpha) * sin(beta), cos(alpha) * sin(beta),
+                   0, cos(alpha), -sin(alpha),
+                   -sin(beta), sin(alpha) * cos(beta), cos(alpha) * cos(beta)));
+   else
+       return Transform3D<T>::DH(alpha, a, d, theta);

             */

            dhjoint_r =
                eps_p[var (_frame) = construct_< DummyFrame > ()] >>    // init _frame
                XMLAttElem_p (
                    "DHJoint",
                    (XMLAtt_p ("name", attrstr_p[var (_frame._name) = arg1]) >>
                     XMLAtt_p ("alpha", real_p[var (_dhparam._alpha) = arg1 * Deg2Rad]) >>
                     XMLAtt_p ("a", real_p[var (_dhparam._a) = arg1]) >>
                     ((XMLAtt_p (
                           "d",
                           real_p[var (_dhparam._dhtype) = Revolute]
                                 [var (_frame._type) = "Revolute"][var (_dhparam._d) = arg1]) >>
                       XMLAtt_p ("offset", real_p[var (_dhparam._offset) = arg1 * Deg2Rad])) |
                      (XMLAtt_p ("theta",
                                 real_p[var (_dhparam._dhtype) = Prismatic]
                                       [var (_frame._type) = "Prismatic"]
                                       [var (_dhparam._theta) = arg1 * Deg2Rad]) >>
                       (XMLAtt_p ("offset", real_p[var (_dhparam._offset) = arg1]))) |
                      (XMLAtt_p ("beta",
                                 real_p[var (_dhparam._dhtype) = Prismatic]
                                       [var (_frame._type) = "Prismatic"]
                                       [var (_dhparam._hgptype) = "parallel"]
                                       [var (_dhparam._beta) = arg1 * Deg2Rad]) >>
                       (XMLAtt_p ("offset", real_p[var (_dhparam._offset) = arg1]))) |
                      (XMLAtt_p ("b",
                                 real_p[var (_dhparam._dhtype) = Revolute]
                                       [var (_frame._type) = "Revolute"]
                                       [var (_dhparam._hgptype) = "parallel"]
                                       [var (_dhparam._b) = arg1]) >>
                       (XMLAtt_p ("offset", real_p[var (_dhparam._offset) = arg1 * Deg2Rad])))

                          ) >>
                     !(XMLAtt_p ("state", jointstate_r)) >>
                     !(XMLAtt_p ("type", attrstr_p[var (_dhparam._type) = arg1])))[SetTransform3D (
                        _dhparam, _frame._transform)][SetDHParam (_dhparam, _frame)],

                    *(jointlimit_r[push_back_a (_frame._limits)] | depend_r |
                      property_p[push_back_a (_frame._properties)] |
                      model_p[push_back_a (_frame._models)]));

            depend_r = XMLAttElem_p ("Depend",
                                     XMLAtt_p ("on", attrstr_p[var (_frame._dependsOn) = arg1]) >>
                                         XMLAtt_p ("gain", real_p[var (_frame._gain) = arg1]) >>
                                         XMLAtt_p ("offset", real_p[var (_frame._offset) = arg1]),
                                     eps_p)[var (_frame._isDepend) = true];

            jointlimit_r =
                eps_p[var (_limit) = construct_< DummyLimit > ()] >>
                (XMLAttElem_p (
                     "PosLimit",
                     !(XMLAtt_p ("refjoint", attrstr_p[var (_limit._refjoint) = arg1])) >>
                             (XMLAtt_p ("min", real_p[var (_limit._min) = arg1]) >>
                              XMLAtt_p ("max",
                                        real_p[var (_limit._max) = arg1]
                                              [var (_limit._type) = PosLimitType])) |
                         (XMLAtt_p ("max", real_p[var (_limit._max) = arg1]) >>
                          XMLAtt_p (
                              "min",
                              real_p[var (_limit._min) = arg1][var (_limit._type) = PosLimitType])),
                     eps_p) |
                 XMLAttElem_p (
                     "VelLimit",
                     !(XMLAtt_p ("refjoint", attrstr_p[var (_limit._refjoint) = arg1])) >>
                         XMLAtt_p (
                             "max",
                             real_p[var (_limit._max) = arg1][var (_limit._type) = VelLimitType]),
                     eps_p) |
                 XMLAttElem_p (
                     "AccLimit",
                     !(XMLAtt_p ("refjoint", attrstr_p[var (_limit._refjoint) = arg1])) >>
                         XMLAtt_p (
                             "max",
                             real_p[var (_limit._max) = arg1][var (_limit._type) = AccLimitType]),
                     eps_p))[jointlimit_r.result_ = var (_limit)];
        }
    };
};

struct XMLWorkcellParser
    : public grammar< XMLWorkcellParser, result_closure< DummyWorkcell >::context_t >
{
  public:
    template< typename ScannerT > struct definition
    {
      private:
        std::vector< std::string > _scope;
        DummyWorkcell _wc;
        rule< ScannerT > workcelldev_r, wc_r, camera_r, guardwrapper_r;
        rule< ScannerT, result_closure< DummyCollisionSetup >::context_t > colsetup_r;
        rule< ScannerT, result_closure< DummyProximitySetup >::context_t > proxsetup_r;
        rule< ScannerT, result_closure< DummyCalibration >::context_t > calibration_r;
        XMLDeviceParser device_p;
        ModelParser model_p;
        FrameParser frame_p;
        DummyCollisionSetup _setup;
        DummyProximitySetup _psetup;
        DummyCalibration _calibration;

      public:
        rule< ScannerT > const start () const { return guardwrapper_r; }

        definition (XMLWorkcellParser const& self) : model_p (_scope), frame_p (_scope)
        {
            guardwrapper_r = XMLErrorHandler::XMLErrorGuard (workcelldev_r)[XMLErrorHandler ()];

            workcelldev_r = (wc_r | device_p[AddDeviceToWorkcell (_wc, _scope)]) >>
                            end_p[self.result_ = var (_wc)];

            wc_r = XMLAttElem_p ("WorkCell",
                                 XMLAtt_p ("name", attrstr_p[var (_wc._name) = arg1]),
                                 *(device_p[AddDeviceToWorkcell (_wc, _scope)] |
                                   frame_p[AddFrameToWorkcell (_wc, _scope)] |
                                   model_p[push_back_a (_wc._models)] | camera_r |
                                   colsetup_r[push_back_a (_wc._colmodels)] |
                                   proxsetup_r[push_back_a (_wc._proxmodels)] |
                                   property_p[push_back_a (_wc._properties)] |
                                   calibration_r[push_back_a (_wc._calibration)]

                                   ));

            colsetup_r = XMLAttElem_p (
                "CollisionSetup",
                XMLAtt_p (
                    "file",
                    attrstr_p[var (_setup._filename) = arg1][var (_setup._scope) = var (_scope)] >>
                        filepos_p[var (_setup._pos) = arg1]),
                eps_p)[colsetup_r.result_ = var (_setup)];

            proxsetup_r = XMLAttElem_p ("ProximitySetup",
                                        XMLAtt_p ("file",
                                                  attrstr_p[var (_psetup._filename) = arg1]
                                                           [var (_psetup._scope) = var (_scope)] >>
                                                      filepos_p[var (_psetup._pos) = arg1]),
                                        eps_p)[proxsetup_r.result_ = var (_psetup)];

            camera_r = XMLAttElem_p ("Camera", XMLAtt_p ("name", attrstr_p), !transform3d_p);
            calibration_r =
                XMLAttElem_p ("Calibration",
                              XMLAtt_p ("file",
                                        attrstr_p[var (_calibration._filename) = arg1]
                                                 [var (_calibration._scope) = var (_scope)] >>
                                            filepos_p[var (_calibration._pos) = arg1]),
                              eps_p)[calibration_r.result_ = var (_calibration)];
        }
    };
};
}    // end of namespace

std::shared_ptr< DummyWorkcell > XMLRWParser::parseWorkcell (const std::string& filename)
{
    std::shared_ptr< std::vector< char > > output (new std::vector< char > ());
    std::shared_ptr< std::vector< std::pair< size_t, file_position > > > filemap (
        new std::vector< std::pair< size_t, file_position > > ());

    if (!rw::loaders::XMLRWPreParser::parse (filename, *output, *filemap)) {
        RW_THROW ("Pre-parsing of file \"" << filename << "\" failed!");
    }

    return XMLRWParser::parseWorkcell (output, filemap);
}

std::shared_ptr< DummyWorkcell > XMLRWParser::parseWorkcell (
    std::shared_ptr< std::vector< char > >& data,
    std::shared_ptr< std::vector< std::pair< size_t, file_position > > >& filemap)
{
    typedef MultipleFileIterator iterator_t;
    iterator_t first (data, filemap);
    iterator_t last (first.end ());

    std::shared_ptr< DummyWorkcell > workcell (new DummyWorkcell);
    XMLWorkcellParser workcell_p;

    parse_info< iterator_t > info = parse (first,
                                           last,
                                           workcell_p[var (*workcell) = arg1],
                                           (space_p | "<!--" >> *(anychar_p - "-->") >> "-->"));

    if (!info.hit) {
        RW_THROW ("Parsing of workcell failed!!");
    }

    return workcell;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Suites
#include <boost/test/unit_test.hpp>

int add(int i, int j)
{
   return i + j;
}

//
// Math
//

BOOST_AUTO_TEST_SUITE(Math)

BOOST_AUTO_TEST_CASE(universeInOrder)
{
   BOOST_CHECK(add(2, 2) == 4);
}

BOOST_AUTO_TEST_SUITE_END()

//
// Physics
//

BOOST_AUTO_TEST_SUITE(Physics)

BOOST_AUTO_TEST_CASE(specialTheory)
{
   int e = 32;
   int m = 2;
   int c = 4;

   BOOST_CHECK(e == m * c * c);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <vector>
#include <Eigen/Core>

namespace kt84 {
    namespace internal {
        template <typename T, int N>
        struct multi_array_helper;
    }

template <typename T, int N>
struct multi_array {
    friend struct internal::multi_array_helper<T, N>;

    typedef Eigen::Matrix<int, N, 1> VectorNi;

    static int total_size(const VectorNi& size) {
        int result = 1;
        for (int i = 0; i < N; ++i) result *= size[i];
        return result;
    }

    // ctor()
    multi_array()
        : size_(VectorNi::Zero())
    {}
    // ctor(size)
    multi_array(const VectorNi& size__)
        : size_(size__)
        , data(total_size(size_))
    {}
    // ctor(size, val)
    multi_array(const VectorNi& size__, const T& val)
        : size_(size__)
        , data(total_size(size_), val)
    {}
    // ctor(sx, ...) specialized for 2D & 3D
    multi_array(int sx, int sy)         { internal::multi_array_helper<T, N>::ctor(sx, sy,     size_, data); }
    multi_array(int sx, int sy, int sz) { internal::multi_array_helper<T, N>::ctor(sx, sy, sz, size_, data); }
    multi_array(int sx, int sy,         const T& val) { internal::multi_array_helper<T, N>::ctor(sx, sy,     val, size_, data); }
    multi_array(int sx, int sy, int sz, const T& val) { internal::multi_array_helper<T, N>::ctor(sx, sy, sz, val, size_, data); }

    void clear() {
        size_.setZero();
        data.clear();
    }

    VectorNi size() const { return size_; }
    int size(int coord) const { return size_[coord]; }
    int total_size() const { return total_size(size_); }
    bool empty() const { return data.empty(); }

    // width() and height() for 2D & 3D, depth() for 3D
    int width () const { return internal::multi_array_helper<T, N>::width (*this); }
    int height() const { return internal::multi_array_helper<T, N>::height(*this); }
    int depth () const { return internal::multi_array_helper<T, N>::depth (*this); }

    int unroll_index(const VectorNi& index) const {
        int result = index[N - 1];
        for (int i = N - 2; i >= 0; --i) {
            result *= size_[i];
            result += index[i];
        }
        return result;
    }
    int unroll_index(int ix, int iy)         const { return internal::multi_array_helper<T, N>::unroll_index(*this, ix, iy); }
    int unroll_index(int ix, int iy, int iz) const { return internal::multi_array_helper<T, N>::unroll_index(*this, ix, iy, iz); }

    // operator(index)
    const T& operator()(const VectorNi& index) const { return data[unroll_index(index)]; }
          T& operator()(const VectorNi& index)       { return data[unroll_index(index)]; }
    // operator(ix, ...) specialized for 2D & 3D
    const T& operator()(int ix, int iy)         const { return internal::multi_array_helper<T, N>::at(*this, ix, iy); }
          T& operator()(int ix, int iy)               { return internal::multi_array_helper<T, N>::at(*this, ix, iy); }
    const T& operator()(int ix, int iy, int iz) const { return internal::multi_array_helper<T, N>::at(*this, ix, iy, iz); }
          T& operator()(int ix, int iy, int iz)       { return internal::multi_array_helper<T, N>::at(*this, ix, iy, iz); }
    // operator[int]
    const T& operator[](int i) const { return data[i]; }
          T& operator[](int i)       { return data[i]; }

    // resize(size)
    void resize(const VectorNi& size__) {
        size_ = size__;
        data.resize(total_size(size_));
    }
    // resize(size, val)
    void resize(const VectorNi& size__, const T& val) {
        size_ = size__;
        data.resize(total_size(size_), val);
    }
    // resize(sx, ...) specialized for 2D & 3D
    void resize(int sx, int sy)         { internal::multi_array_helper<T, N>::resize(*this, sx, sy); }
    void resize(int sx, int sy, int sz) { internal::multi_array_helper<T, N>::resize(*this, sx, sy, sz); }
    void resize(int sx, int sy,         const T& val) { internal::multi_array_helper<T, N>::resize(*this, sx, sy, val); }
    void resize(int sx, int sy, int sz, const T& val) { internal::multi_array_helper<T, N>::resize(*this, sx, sy, sz, val); }

    // begin/end
    typedef typename std::vector<T>::iterator iterator;
    typedef typename std::vector<T>::const_iterator const_iterator;
    typename iterator       begin()       { return data.begin(); }
    typename iterator       end  ()       { return data.end  (); }
    typename const_iterator begin() const { return data.begin(); }
    typename const_iterator end  () const { return data.end  (); }
    typename const_iterator cbegin() const { return begin(); }
    typename const_iterator cend  () const { return end  (); }

private:
    VectorNi size_;
    std::vector<T> data;
};

    namespace internal {
        template <typename T>
        struct multi_array_helper<T, 2> {
            static void ctor(int sx, int sy, Eigen::Vector2i& size, std::vector<T>& data) {
                size = Eigen::Vector2i(sx, sy);
                data.resize(multi_array<T, 2>::total_size(size));
            }
            static void ctor(int sx, int sy, const T& val, Eigen::Vector2i& size, std::vector<T>& data) {
                size = Eigen::Vector2i(sx, sy);
                data.resize(multi_array<T, 2>::total_size(size), val);
            }
            static int width (const multi_array<T, 2>& m) { return m.size_.x(); }
            static int height(const multi_array<T, 2>& m) { return m.size_.y(); }
            static int unroll_index(const multi_array<T, 2>& m, int ix, int iy) { return m.unroll_index(Eigen::Vector2i(ix, iy)); }
            static const T& at(const multi_array<T, 2>& m, int ix, int iy) { return m(Eigen::Vector2i(ix, iy)); }
            static       T& at(      multi_array<T, 2>& m, int ix, int iy) { return m(Eigen::Vector2i(ix, iy)); }
            static void resize(multi_array<T, 2>& m, int sx, int sy)               { m.resize(Eigen::Vector2i(sx, sy)); }
            static void resize(multi_array<T, 2>& m, int sx, int sy, const T& val) { m.resize(Eigen::Vector2i(sx, sy), val); }
        };
        template <typename T>
        struct multi_array_helper<T, 3> {
            static void ctor(int sx, int sy, int sz, Eigen::Vector3i& size, std::vector<T>& data) {
                size = Eigen::Vector3i(sx, sy, sz);
                data.resize(multi_array<T, 3>::total_size(size));
            }
            static void ctor(int sx, int sy, int sz, const T& val, Eigen::Vector3i& size, std::vector<T>& data) {
                size = Eigen::Vector3i(sx, sy, sz);
                data.resize(multi_array<T, 3>::total_size(size), val);
            }
            static int width (const multi_array<T, 3>& m) { return m.size_.x(); }
            static int height(const multi_array<T, 3>& m) { return m.size_.y(); }
            static int depth (const multi_array<T, 3>& m) { return m.size_.z(); }
            static int unroll_index(const multi_array<T, 3>& m, int ix, int iy, int iz) { return m.unroll_index(Eigen::Vector3i(ix, iy, iz)); }
            static const T& at(const multi_array<T, 3>& m, int ix, int iy, int iz) { return m(Eigen::Vector3i(ix, iy, iz)); }
            static       T& at(      multi_array<T, 3>& m, int ix, int iy, int iz) { return m(Eigen::Vector3i(ix, iy, iz)); }
            static void resize(multi_array<T, 3>& m, int sx, int sy, int sz)               { m.resize(Eigen::Vector3i(sx, sy, sz)); }
            static void resize(multi_array<T, 3>& m, int sx, int sy, int sz, const T& val) { m.resize(Eigen::Vector2i(sx, sy, sz), val); }
        };
    }
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief Implementation of \link kwiver::vital::camera_perspective
 * camera_perspective \endlink class
 */

#include <vital/types/camera_perspective.h>
#include <vital/io/eigen_io.h>
#include <vital/types/matrix.h>
#include <Eigen/Geometry>

#include <iomanip>

namespace kwiver {
namespace vital {

camera_perspective
::camera_perspective()
  : m_logger( kwiver::vital::get_logger( "vital.camera_perspective" ) )
{
}

/// Convert to a 3x4 homogeneous projection matrix
matrix_3x4d
camera_perspective
::as_matrix() const
{
  matrix_3x4d P;
  matrix_3x3d R( this->rotation().matrix() );
  matrix_3x3d K( this->intrinsics()->as_matrix() );
  vector_3d t( this->translation() );
  P.block< 3, 3 > ( 0, 0 ) = R;
  P.block< 3, 1 > ( 0, 3 ) = t;
  return K * P;
}

/// Convert to a 3x4 pose matrix (no intrinsics)
matrix_3x4d
camera_perspective
::pose_matrix() const
{
  matrix_3x4d P;
  matrix_3x3d R(this->rotation().matrix());
  vector_3d t(this->translation());
  P.block< 3, 3 >(0, 0) = R;
  P.block< 3, 1 >(0, 3) = t;
  return P;
}

/// Project a 3D point into a 2D image point
vector_2d
camera_perspective
::project( const vector_3d& pt ) const
{
  return this->intrinsics()->map( this->rotation() * ( pt - this->center() ));
}

/// Compute the distance of the 3D point to the image plane
double
camera_perspective
::depth(const vector_3d& pt) const
{
  return (this->rotation() * (pt - this->center())).z();
}

/// output stream operator for a base class camera_perspective
std::ostream&
operator<<( std::ostream& s, const camera_perspective& c )
{
  using std::setprecision;
  std::vector<double> dc = c.intrinsics()->dist_coeffs();
  Eigen::VectorXd d = Eigen::VectorXd::Map(dc.data(), dc.size());
  // if no distortion coefficients, create a zero entry as a place holder
  if ( d.rows() == 0 )
  {
    d.resize( 1 );
    d[0] = 0.0;
  }
  s << setprecision( 12 ) << c.intrinsics()->as_matrix() << "\n\n"
    << setprecision( 12 ) << c.rotation().matrix() << "\n\n"
    << setprecision( 12 ) << c.translation().transpose() << "\n\n"
    << setprecision( 12 ) << d.transpose() << "\n";
  return s;
}

/// Rotate the camera about its center such that it looks at the given point.
void
simple_camera_perspective
::look_at( const vector_3d &stare_point,
           const vector_3d &up_direction )
{
  // a unit vector in the up direction
  const vector_3d up = up_direction.normalized();
  // a unit vector in the look direction (camera Z-axis)
  const vector_3d z = ( stare_point - get_center() ).normalized();

  // the X-axis of the camera is perpendicular to up and z
  vector_3d x = -up.cross( z );
  double x_mag = x.norm();

  // if the cross product magnitude is small then the up and z vectors are
  // nearly parallel and the up direction is poorly defined.
  if ( x_mag < 1e-4 )
  {
    LOG_WARN( m_logger,
              "simple_camera_perspective::look_at up_direction nearly parallel"
              " with the look direction" );
  }

  x /= x_mag;
  vector_3d y = z.cross( x ).normalized();

  matrix_3x3d R;
  R << x.x(), x.y(), x.z(),
    y.x(), y.y(), y.z(),
    z.x(), z.y(), z.z();

  this->set_rotation( rotation_d ( R ) );
}

/// Create a clone of this camera that is rotated to look at the given point
camera_perspective_sptr
simple_camera_perspective
::clone_look_at( const vector_3d &stare_point,
                 const vector_3d &up_direction ) const
{
  auto c_sptr = std::dynamic_pointer_cast<camera_perspective>(this->clone());
  dynamic_cast<simple_camera_perspective *>(c_sptr.get())->look_at( stare_point,
                                                                    up_direction );
  return c_sptr;
}

/// input stream operator for a camera intrinsics
std::istream&
operator>>( std::istream& s, simple_camera_perspective& k )
{
  matrix_3x3d K, R;
  vector_3d t;
  s >> K >> R >> t;

  double dVal;
  std::vector<double> dValues;

  while (s >> dVal)
  {
    dValues.push_back(dVal);
  }

  Eigen::VectorXd d(dValues.size());

  for (size_t i = 0; i < dValues.size(); ++i)
  {
    d(i) =  dValues[i];
  }

  // a single 0 in d is used as a place holder,
  // if a single 0 was loaded then clear d
  if ( ( d.rows() == 1 ) && ( d[0] ==  0.0 ) )
  {
    d.resize( 0 );
  }
  k.set_intrinsics( camera_intrinsics_sptr(new simple_camera_intrinsics( K, d ) ) );
  k.set_rotation( rotation_d ( R ) );
  k.set_translation( t );
  return s;
}

} } // end namespace


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/bit_ops.hpp>

int main()
{
    return boost::bit_ops::countr_zero(192u);
}


/*
 * Copyright (c) 2015 University of Basel
 * 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 project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <boost/program_options.hpp>

#include <itkBoundingBox.h>
#include <itkCompositeTransform.h>
#include <itkSignedMaurerDistanceMapImageFilter.h>
#include <itkLBFGSOptimizer.h>
#include <itkLinearInterpolateImageFunction.h>
#include <itkMesh.h>
#include <itkPointSetToImageFilter.h>
#include <itkPointSetToImageRegistrationMethod.h>
#include <itkPointsLocator.h>
#include <itkReducedVarianceModelBuilder.h>
#include <itkStandardMeshRepresenter.h>
#include <itkStatismoIO.h>
#include <itkStatisticalModel.h>
#include <itkStatisticalShapeModelTransform.h>
#include <itkVersorRigid3DTransform.h>

#include "utils/itkPenalizingMeanSquaresPointSetToImageMetric.h"
#include "utils/statismo-build-models-utils.h"
#include "utils/statismo-fitting-utils.h"

namespace po = boost::program_options;
using namespace std;

const unsigned Dimensions = 3;
typedef itk::Mesh<float, Dimensions> DataType;
typedef itk::Image<float, Dimensions> DistanceImageType;
typedef itk::StatisticalModel<DataType> StatisticalModelType;

struct programOptions {
    bool bDisplayHelp;
    bool bPrintFittingInformation;

    string strInputModelFileName;
    string strInputTargetMeshFileName;

    string strOutputFittedMeshFileName;
    string strOutputProjectedMeshFileName;

    double dRegularizationWeight;
    unsigned uNumberOfIterations;
    string strInputFixedLandmarksFileName;
    string strInputMovingLandmarksFileName;
    double dLandmarksVariance;
};

po::options_description initializeProgramOptions(programOptions& poParameters);
bool isOptionsConflictPresent(programOptions& opt);
void fitMesh(programOptions opt, ConsoleOutputSilencer* pCOSilencer);

int main(int argc, char** argv) {
    programOptions poParameters;
    po::options_description optAllOptions = initializeProgramOptions(poParameters);

    po::variables_map vm;
    try {
        po::parsed_options parsedOptions = po::command_line_parser(argc, argv).options(optAllOptions).run();
        po::store(parsedOptions, vm);
        po::notify(vm);
    } catch (po::error& e) {
        cerr << "An exception occurred while parsing the Command line:"<<endl;
        cerr << e.what() << endl << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    if (poParameters.bDisplayHelp == true) {
        cout << optAllOptions << endl;
        return EXIT_SUCCESS;
    }
    if (isOptionsConflictPresent(poParameters) == true)	{
        cerr << "A conflict in the options exists or insufficient options were set." << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    ConsoleOutputSilencer coSilencer;
    try {
        fitMesh(poParameters, &coSilencer);
    } catch (ifstream::failure & e) {
        coSilencer.enableOutput();
        cerr << "Could not read a file:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    } catch (itk::ExceptionObject & e) {
        coSilencer.enableOutput();
        cerr << "Could not fit the model:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}

bool isOptionsConflictPresent(programOptions& opt) {
    //if one set of the landmarks-file is provided, then both have to be provided (-> use XOR)
    if (((opt.strInputFixedLandmarksFileName != "") ^ (opt.strInputMovingLandmarksFileName != "")) == true) {
        return true;
    }

    //at least one of the outputs needs to be set or the fitting information has to be printed
    if (opt.strOutputFittedMeshFileName == "" && opt.strOutputProjectedMeshFileName == "") {
        return true;
    }

    if (opt.strInputModelFileName == "" || opt.strInputTargetMeshFileName == "") {
        return true;
    }

    return false;
}

DistanceImageType::Pointer computeDistanceImageForMesh(DataType::Pointer pMesh, const unsigned uDistImageResolution = 256) {
    // Compute a bounding box around the reference shape
    typedef  itk::BoundingBox<int, Dimensions, float, DataType::PointsContainer> BoundingBoxType;
    BoundingBoxType::Pointer pBoundingBox = BoundingBoxType::New();
    pBoundingBox->SetPoints(pMesh->GetPoints());
    pBoundingBox->ComputeBoundingBox();

    // Compute a binary image from the point set, which is as large as the bounding box plus a margin.
    typedef itk::Image< unsigned char, Dimensions > BinaryImageType;
    typedef itk::PointSet<float, Dimensions  > PointSetType;
    typedef itk::PointSetToImageFilter<PointSetType, BinaryImageType> PointsToImageFilterType;
    PointsToImageFilterType::Pointer pointsToImageFilter = PointsToImageFilterType::New();
    pointsToImageFilter->SetInput(pMesh);
    BinaryImageType::SpacingType spacing;
    BinaryImageType::SpacingType margin;
    BinaryImageType::PointType origin = pBoundingBox->GetMinimum();
    BinaryImageType::SpacingType diff = pBoundingBox->GetMaximum() - pBoundingBox->GetMinimum();
    BinaryImageType::SizeType size;

    for (unsigned i = 0; i < Dimensions; ++i) {
        margin[i] = diff[i] * 0.1; // 10 % margin on each side
        origin[i] -= margin[i];
        size[i] = uDistImageResolution + margin[i];
        spacing[i] = (diff[i] + 2.0 * margin[i]) / uDistImageResolution;
    }

    pointsToImageFilter->SetSpacing(spacing);
    pointsToImageFilter->SetOrigin(origin);
    pointsToImageFilter->SetSize(size);
    pointsToImageFilter->Update();

    // compute a distance map to the points in the pointset
    BinaryImageType::Pointer pBinaryImage = pointsToImageFilter->GetOutput();
    typedef itk::SignedMaurerDistanceMapImageFilter<BinaryImageType, DistanceImageType> DistanceFilterType;
    DistanceFilterType::Pointer distanceFilter = DistanceFilterType::New();
    distanceFilter->SetInput(pBinaryImage);
    distanceFilter->Update();
    DistanceImageType::Pointer pDistanceImage = distanceFilter->GetOutput();
    return pDistanceImage;
}

void saveMesh(DataType::Pointer pData, const string& strFileName) {
    typedef itk::MeshFileWriter<DataType> DataWriterType;
    DataWriterType::Pointer pDataWriter = DataWriterType::New();
    pDataWriter->SetFileName(strFileName);
    pDataWriter->SetInput(pData);
    pDataWriter->Update();
}

template<class PointsLocatorType>
DataType::Pointer projectOnTargetMesh(DataType::Pointer pMesh, DataType::Pointer pTargetMesh) {
    typename PointsLocatorType::Pointer ptLocator = PointsLocatorType::New();
    ptLocator->SetPoints(pTargetMesh->GetPoints());
    ptLocator->Initialize();
    DataType::Pointer projectedMesh = cloneMesh<DataType>(pMesh);
    for (DataType::PointsContainer::Iterator pointIter = projectedMesh->GetPoints()->Begin(); pointIter != projectedMesh->GetPoints()->End(); ++pointIter) {
        unsigned uClosestPointId = ptLocator->FindClosestPoint(pointIter->Value());
        pointIter->Value() = pTargetMesh->GetPoint(uClosestPointId);
    }
    return projectedMesh;
}

void fitMesh(programOptions opt, ConsoleOutputSilencer* pCOSilencer) {
    typedef itk::MeshFileReader<DataType> MeshReaderType;
    MeshReaderType::Pointer pMeshReader = MeshReaderType::New();
    pMeshReader->SetFileName(opt.strInputTargetMeshFileName.c_str());
    pMeshReader->Update();
    DataType::Pointer pTargetMesh = pMeshReader->GetOutput();

    typedef itk::StandardMeshRepresenter<float, Dimensions> RepresenterType;
    RepresenterType::Pointer pRepresenter = RepresenterType::New();
    StatisticalModelType::Pointer pModel = StatisticalModelType::New();
    pModel = itk::StatismoIO<DataType>::LoadStatisticalModel(pRepresenter.GetPointer(),
                                                             opt.strInputModelFileName.c_str());

    StatisticalModelType::Pointer pConstrainedModel;
    typedef itk::StatisticalShapeModelTransform<DataType, double, Dimensions> StatisticalModelTransformType;
    typedef itk::Transform<double, Dimensions, Dimensions> TransformType;
    TransformType::Pointer pTransform;

    typedef itk::PointsLocator< DataType::PointsContainer > PointsLocatorType;
    if (opt.strInputFixedLandmarksFileName == "") {
        typedef  itk::BoundingBox<int, Dimensions, float, DataType::PointsContainer> BoundingBoxType;
        //Compute bounding box of model mesh
        BoundingBoxType::Pointer pModelMeshBox = BoundingBoxType::New();
        pModelMeshBox->SetPoints(pModel->GetRepresenter()->GetReference()->GetPoints());
        pModelMeshBox->ComputeBoundingBox();

        //Compute bounding box of target mesh
        BoundingBoxType::Pointer pTargetMeshBox = BoundingBoxType::New();
        pTargetMeshBox->SetPoints(pTargetMesh->GetPoints());
        pTargetMeshBox->ComputeBoundingBox();

        pConstrainedModel = pModel;
        StatisticalModelTransformType::Pointer pModelTransform = StatisticalModelTransformType::New();
        pModelTransform->SetStatisticalModel(pModel);
        pModelTransform->SetIdentity();

        //No Landmarks are available: we also have to allow rotation and translation.
        typedef itk::VersorRigid3DTransform<double> RotationAndTranslationTransformType;
        RotationAndTranslationTransformType::Pointer pRotationAndTranslationTransform = RotationAndTranslationTransformType::New();
        pRotationAndTranslationTransform->SetIdentity();

        RotationAndTranslationTransformType::CenterType center = pModelMeshBox->GetCenter();
        pRotationAndTranslationTransform->SetCenter(center);

        RotationAndTranslationTransformType::TranslationType translation = pTargetMeshBox->GetCenter() - pModelMeshBox->GetCenter();
        pRotationAndTranslationTransform->SetTranslation(translation);

        typedef itk::CompositeTransform<double, Dimensions> CompositeTransformType;
        CompositeTransformType::Pointer pCompositeTransform = CompositeTransformType::New();
        pCompositeTransform->AddTransform(pRotationAndTranslationTransform);
        pCompositeTransform->AddTransform(pModelTransform);
        pCompositeTransform->SetAllTransformsToOptimizeOn();

        pTransform = pCompositeTransform;
    } else {
        pConstrainedModel = buildPosteriorShapeModel<DataType, StatisticalModelType, PointsLocatorType>(pModel, opt.strInputFixedLandmarksFileName, opt.strInputMovingLandmarksFileName, opt.dLandmarksVariance);
        StatisticalModelTransformType::Pointer pModelTransform = StatisticalModelTransformType::New();
        pModelTransform->SetStatisticalModel(pConstrainedModel);
        pModelTransform->SetIdentity();
        pTransform = pModelTransform;
    }

    DistanceImageType::Pointer pDistanceImage = computeDistanceImageForMesh(pTargetMesh);

    typedef itk::LBFGSOptimizer OptimizerType;
    OptimizerType::Pointer pOptimizer = OptimizerType::New();
    initializeOptimizer<OptimizerType>(pOptimizer, opt.uNumberOfIterations, pModel->GetNumberOfPrincipalComponents(), pTransform->GetNumberOfParameters(), opt.bPrintFittingInformation, pCOSilencer);

    typedef itk::LinearInterpolateImageFunction<DistanceImageType, double> InterpolatorType;
    InterpolatorType::Pointer pInterpolator = InterpolatorType::New();

    typedef itk::PointSet<float, Dimensions > PointSetType;
    typedef itk::PenalizingMeanSquaresPointSetToImageMetric<PointSetType, DistanceImageType> MetricType;
    MetricType::Pointer pMetric = MetricType::New();
    pMetric->SetRegularizationParameter(opt.dRegularizationWeight);
    pMetric->SetNumberOfModelComponents(pConstrainedModel->GetNumberOfPrincipalComponents());

    DataType::Pointer pReference= pModel->GetRepresenter()->GetReference();
    PointSetType::Pointer pFixedPointSet = PointSetType::New();
    pFixedPointSet->SetPoints(pReference->GetPoints());
    PointSetType::PointDataContainer::Pointer pPoints = PointSetType::PointDataContainer::New();
    pPoints->Reserve(pReference->GetNumberOfPoints());
    for (PointSetType::PointDataContainer::Iterator it = pPoints->Begin(); it != pPoints->End(); ++it) {
        it->Value() = 0;
    }
    pFixedPointSet->SetPointData(pPoints);

    typedef itk::PointSetToImageRegistrationMethod<PointSetType,DistanceImageType > RegistrationFilterType;
    RegistrationFilterType::Pointer pRegistration = RegistrationFilterType::New();
    pRegistration->SetInitialTransformParameters(pTransform->GetParameters());
    pRegistration->SetMetric(pMetric);
    pRegistration->SetInterpolator(pInterpolator);
    pRegistration->SetOptimizer(pOptimizer);
    pRegistration->SetTransform(pTransform);
    pRegistration->SetFixedPointSet(pFixedPointSet);
    pRegistration->SetMovingImage(pDistanceImage);

    pCOSilencer->disableOutput();
    pRegistration->Update();
    pCOSilencer->enableOutput();

    typedef itk::TransformMeshFilter<DataType, DataType, TransformType> TransformMeshFilterType;
    TransformMeshFilterType::Pointer pTransformMeshFilter = TransformMeshFilterType::New();
    pTransformMeshFilter->SetInput(pModel->GetRepresenter()->GetReference());
    pTransformMeshFilter->SetTransform(pTransform);
    pTransformMeshFilter->Update();

    DataType::Pointer pFittedMesh = pTransformMeshFilter->GetOutput();
    if(opt.strOutputFittedMeshFileName != "") {
        saveMesh(pFittedMesh, opt.strOutputFittedMeshFileName);
    }
    if(opt.strOutputProjectedMeshFileName != "") {
        DataType::Pointer pProjectedMesh = projectOnTargetMesh<PointsLocatorType>(pFittedMesh, pTargetMesh);
        saveMesh(pProjectedMesh, opt.strOutputProjectedMeshFileName);
    }
}

po::options_description initializeProgramOptions(programOptions& poParameters) {
    po::options_description optMandatory("Mandatory options");
    optMandatory.add_options()

    ("input-model,i", po::value<string>(&poParameters.strInputModelFileName), "The path to the model file.")
    ("input-targetmesh,t", po::value<string>(&poParameters.strInputTargetMeshFileName), "The path to the target mesh.")
    ("number-of-iterations,n", po::value<unsigned>(&poParameters.uNumberOfIterations)->default_value(100), "Number of iterations")
    ("regularization-weight,w", po::value<double>(&poParameters.dRegularizationWeight), "This is the regularization weight to make sure the model parameters don't don't get too big while fitting.")
    ;

    po::options_description optOutput("Mandatory Output options (set at least one)");
    optOutput.add_options()
    ("output-fit,o", po::value<string>(&poParameters.strOutputFittedMeshFileName), "Name of the output file where the fitted mesh will be written to.")
    ("output-projected,j", po::value<string>(&poParameters.strOutputProjectedMeshFileName), "Name of the output file where the projected mesh will be written to.")
    ;

    po::options_description optLandmarks("Landmarks (optional - if you set one you have to set all)");
    optLandmarks.add_options()
    ("fixed-landmarks,f", po::value<string>(&poParameters.strInputFixedLandmarksFileName), "Name of the file where the fixed Landmarks are saved.")
    ("moving-landmarks,m", po::value<string>(&poParameters.strInputMovingLandmarksFileName), "Name of the file where the moving Landmarks are saved.")
    ("landmarks-variance,v", po::value<double>(&poParameters.dLandmarksVariance)->default_value(1), "The variance that will be used to build the posterior model.")
    ;

    po::options_description optAdditional("Optional options");
    optAdditional.add_options()
    ("print-fitting-information,p", po::bool_switch(&poParameters.bPrintFittingInformation), "Prints information (the parameters, metric score and the iteration count) with each iteration while fitting.")
    ("help,h", po::bool_switch(&poParameters.bDisplayHelp), "Display this help message")
    ;

    po::options_description optAllOptions;
    optAllOptions.add(optMandatory).add(optOutput).add(optLandmarks).add(optAdditional);
    return optAllOptions;
}