/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
// Probabilistic Reasoning Library (PRL)
// Copyright 2009 (see AUTHORS.txt for a list of contributors)
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef TURI_STL_UTIL_HPP
#define TURI_STL_UTIL_HPP

#include <set>
#include <map>
#include <vector>
#include <algorithm>
#include <iterator>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <core/logging/assertions.hpp>
#include <boost/exception/detail/is_output_streamable.hpp>

// #include <core/storage/serialization/serialize.hpp>
// #include <core/storage/serialization/set.hpp>
// #include <core/storage/serialization/map.hpp>

namespace turi {

  /**
   * \ingroup util
   * \addtogroup set_and_map Set And Map Utilities
   * \brief Some mathematical Set and Map routines.
   * \{
   */
  // Functions on sets
  //============================================================================

  /**
   * computes the union of two sets.
   */
  template <typename T>
  std::set<T> set_union(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_union(a.begin(), a.end(),
                   b.begin(), b.end(),
                   std::inserter(output, output.begin()));
    return output;
  }

  /**
   * computes the union of a set and a value.
   */
  template <typename T>
  std::set<T> set_union(const std::set<T>& a, const T& b) {
    std::set<T> output = a;
    output.insert(b);
    return output;
  }

  /**
   * computes the intersect of two sets.
   */
  template <typename T>
  std::set<T> set_intersect(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_intersection(a.begin(), a.end(),
                          b.begin(), b.end(),
                          std::inserter(output, output.begin()));
    return output;
  }

  /**
   * computes the difference of two sets.
   */
  template <typename T>
  std::set<T> set_difference(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_difference(a.begin(), a.end(),
                        b.begin(), b.end(),
                        std::inserter(output, output.begin()));
    return output;
  }

  /**
   * Subtract a value from a set
   */
  template <typename T>
  std::set<T> set_difference(const std::set<T>& a, const T& b) {
    std::set<T> output = a;
    output.erase(b);
    return output;
  }

  /**
   * Partitions a set with a different set.
   * Returns 2 sets: <s in partition, s not in partition>
   */
  template <typename T>
  std::pair<std::set<T>,std::set<T> >
  set_partition(const std::set<T>& s, const std::set<T>& partition) {
    std::set<T> a, b;
    a = set_intersect(s, partition);
    b = set_difference(s, partition);
    return std::make_pair(a, b);
  }

  /**
   * Returns true if the two sets are disjoint
   */
  template <typename T>
  bool set_disjoint(const std::set<T>& a, const std::set<T>& b) {
    return (intersection_size(a,b) == 0);
  }

  /**
   * Returns true if the two sets are equal
   */
  template <typename T>
  bool set_equal(const std::set<T>& a, const std::set<T>& b) {
    if (a.size() != b.size()) return false;
    return a == b; // defined in <set>
  }

  /**
   * Returns true if b is included in a
   */
  template <typename T>
  bool includes(const std::set<T>& a, const std::set<T>& b) {
    return std::includes(a.begin(), a.end(), b.begin(), b.end());
  }

  /**
   * Returns true if $a \subseteq b$
   */
  template <typename T>
  bool is_subset(const std::set<T>& a, const std::set<T>& b) {
    return includes(b, a);
  }

  /**
   * Returns true if $b \subseteq a$
   */
  template <typename T>
  bool is_superset(const std::set<T>& a,const std::set<T>& b) {
    return includes(a, b);
  }

  /*
   * \internal
   * Prints a container.
   */
  template <typename Container>
  std::ostream& print_range(std::ostream& out, const Container& c,
                   const std::string& left, const std::string& sep,const std::string& right) {

    out << left;

    for(auto it = c.begin();;) {
      if(it == c.end())
        break;

      out << *it;

      ++it;

      if(it != c.end())
        out << sep;
    }
    out << right << std::endl;

    return out;
  }

  /**
   * Writes a human representation of the set to the supplied stream.
   */
  template <typename T>
  typename boost::enable_if_c<boost::is_output_streamable<T>::value,
           std::ostream&>::type
  operator<<(std::ostream& out, const std::set<T>& s) {
    return print_range(out, s, "{", ", ", "}");
  }

  /**
   * Writes a human representation of a vector to the supplied stream.
   */
  template <typename T>
  typename boost::enable_if_c<boost::is_output_streamable<T>::value,
           std::ostream&>::type
  operator<<(std::ostream& out, const std::vector<T>& v) {
    return print_range(out, v, "[", ", ", "]");
  }

  // Functions on maps
  //============================================================================

  /**
   * constant lookup in a map. assertion failure of key not found in map
   */
  template <typename Key, typename T>
  const T& safe_get(const std::map<Key, T>& map,
                    const Key& key) {
    typedef typename std::map<Key, T>::const_iterator iterator;
    iterator iter = map.find(key);
    ASSERT_TRUE(iter != map.end());
    return iter->second;
  } // end of safe_get

  /**
   * constant lookup in a map. If key is not found in map,
   * 'default_value' is returned. Note that this can't return a reference
   * and must return a copy
   */
  template <typename Key, typename T>
  const T safe_get(const std::map<Key, T>& map,
                    const Key& key, const T default_value) {
    typedef typename std::map<Key, T>::const_iterator iterator;
    iterator iter = map.find(key);
    if (iter == map.end())   return default_value;
    else return iter->second;
  } // end of safe_get

  /**
   * Transform each key in the map using the key_map
   * transformation. The resulting map will have the form
   * output[key_map[i]] = map[i]
   */
  template <typename OldKey, typename NewKey, typename T>
  std::map<NewKey, T>
  rekey(const std::map<OldKey, T>& map,
        const std::map<OldKey, NewKey>& key_map) {
    std::map<NewKey, T> output;
    typedef std::pair<OldKey, T> pair_type;
    for(const pair_type& pair: map) {
      output[safe_get(key_map, pair.first)] = pair.second;
    }
    return output;
  }

  /**
   * Transform each key in the map using the key_map
   * transformation. The resulting map will have the form
   output[i] = remap[map[i]]
  */
  template <typename Key, typename OldT, typename NewT>
  std::map<Key, NewT>
  remap(const std::map<Key, OldT>& map,
        const std::map<OldT, NewT>& val_map) {
    std::map<Key, NewT> output;
    typedef std::pair<Key, OldT> pair_type;
    for(const pair_type& pair: map) {
      output[pair.first] = safe_get(val_map, pair.second);
    }
    return output;
  }

  /**
   * Inplace version of remap
   */
  template <typename Key, typename T>
  void remap(std::map<Key, T>& map,
             const std::map<T, T>& val_map) {
    typedef std::pair<Key, T> pair_type;
    for(pair_type& pair: map) {
      pair.second = safe_get(val_map, pair.second);
    }
  }

  /**
   * Computes the union of two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_union(const std::map<Key, T>& a,
            const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    std::set_union(a.begin(), a.end(),
                   b.begin(), b.end(),
                   std::inserter(output, output.begin()),
                   output.value_comp());
    return output;
  }

  /**
   * Computes the intersection of two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_intersect(const std::map<Key, T>& a,
                const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    // compute the intersection
    std::set_intersection(a.begin(), a.end(),
                          b.begin(), b.end(),
                          std::inserter(output, output.begin()),
                          output.value_comp());
    return output;
  }

  /**
   * Returns the entries of a map whose keys show up in the set keys
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_intersect(const std::map<Key, T>& m,
                const std::set<Key>& keys) {
    std::map<Key, T> output;
    for(const Key& key: keys) {
      typename std::map<Key,T>::const_iterator it = m.find(key);
      if (it != m.end())
        output[key] = it->second;
    }
    return output;
  }

  /**
   * Computes the difference between two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_difference(const std::map<Key, T>& a,
                 const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    // compute the intersection
    std::set_difference(a.begin(), a.end(),
                        b.begin(), b.end(),
                        std::inserter(output, output.begin()),
                        output.value_comp());
    return output;
  }

  /**
   * Returns the set of keys in a map
   */
  template <typename Key, typename T>
  std::set<Key> keys(const std::map<Key, T>& map) {
    std::set<Key> output;
    typedef std::pair<Key, T> pair_type;
    for(const pair_type& pair: map) {
      output.insert(pair.first);
    }
    return output;
  }

  /**
   * Get the set of keys in a map as a vector
   */
  template <typename Key, typename T>
  std::vector<Key> keys_as_vector(const std::map<Key, T>& map) {
    std::vector<Key> output(map.size());
    typedef std::pair<Key, T> pair_type;
    size_t i = 0;
    for(const pair_type& pair: map) {
      output[i++] = pair.first;
    }
    return output;
  }

  /**
   * Gets the values from a map
   */
  template <typename Key, typename T>
  std::set<T> values(const std::map<Key, T>& map) {
    std::set<T> output;
    typedef std::pair<Key, T> pair_type;
    for(const pair_type& pair: map) {
      output.insert(pair.second);
    }
    return output;
  }

  /**
   * Gets a subset of values from a map
   */
  template <typename Key, typename T>
  std::vector<T> values(const std::map<Key, T>& m,
                        const std::set<Key>& keys) {
    std::vector<T> output;

    for(const Key &i: keys) {
      output.push_back(safe_get(m, i));
    }
    return output;
  }

  /**
   * Gets a subset of values from a map
   */
  template <typename Key, typename T>
  std::vector<T> values(const std::map<Key, T>& m,
                        const std::vector<Key>& keys) {
    std::vector<T> output;
    for(const Key &i: keys) {
      output.push_back(safe_get(m, i));
    }
    return output;
  }

  /** Creates an identity map (a map from elements to themselves)
   */
  template <typename Key>
  std::map<Key, Key> make_identity_map(const std::set<Key>& keys) {
    std::map<Key, Key> m;
    for(const Key& key: keys)
      m[key] = key;
    return m;
  }

  //! Writes a map to the supplied stream.
  template <typename Key, typename T>
  std::ostream& operator<<(std::ostream& out, const std::map<Key, T>& m) {
    out << "{";
    for (typename std::map<Key, T>::const_iterator it = m.begin();
         it != m.end();) {
      out << it->first << "-->" << it->second;
      if (++it != m.end()) out << " ";
    }
    out << "}";
    return out;
  }

  /** Removes white space (space and tabs) from the beginning and end of str,
      returning the resultant string
  */
  inline std::string trim(const std::string& str) {
    std::string::size_type pos1 = str.find_first_not_of(" \t");
    std::string::size_type pos2 = str.find_last_not_of(" \t");
    return str.substr(pos1 == std::string::npos ? 0 : pos1,
                      pos2 == std::string::npos ? str.size()-1 : pos2-pos1+1);
  }

  /**
  * Convenience function for using std streams to convert anything to a string
  */
  template<typename T>
  std::string tostr(const T& t) {
    std::stringstream strm;
    strm << t;
    return strm.str();
  }

  /**
  * Convenience function for using std streams to convert a string to anything
  */
  template<typename T>
  T fromstr(const std::string& str) {
    std::stringstream strm(str);
    T elem;
    strm >> elem;
    ASSERT_FALSE(strm.fail());
    return elem;
  }

  /**
  Returns a string representation of the number,
  padded to 'npad' characters using the pad_value character
  */
  inline std::string pad_number(const size_t number,
                                const size_t npad,
                                const char pad_value = '0') {
    std::stringstream strm;
    strm << std::setw((int)npad) << std::setfill(pad_value)
         << number;
    return strm.str();
  }

  // inline std::string change_suffix(const std::string& fname,
  //                                  const std::string& new_suffix) {
  //   size_t pos = fname.rfind('.');
  //   assert(pos != std::string::npos);
  //   const std::string new_base(fname.substr(0, pos));
  //   return new_base + new_suffix;
  // } // end of change_suffix

  /**
  Using splitchars as delimiters, splits the string into a vector of strings.
  if auto_trim is true, trim() is called on all the extracted strings
  before returning.
  */
  inline std::vector<std::string> strsplit(const std::string& str,
                                           const std::string& splitchars,
                                           const bool auto_trim = false) {
    std::vector<std::string> tokens;
    for(size_t beg = 0, end = 0; end != std::string::npos; beg = end+1) {
      end = str.find_first_of(splitchars, beg);
      if(auto_trim) {
        if(end - beg > 0) {
          std::string tmp = trim(str.substr(beg, end - beg));
          if(!tmp.empty()) tokens.push_back(tmp);
        }
      } else tokens.push_back(str.substr(beg, end - beg));
    }
    return tokens;
    // size_t pos = 0;
    // while(1) {
    //   size_t nextpos = s.find_first_of(splitchars, pos);
    //   if (nextpos != std::string::npos) {
    //     ret.push_back(s.substr(pos, nextpos - pos));
    //     pos = nextpos + 1;
    //   } else {
    //     ret.push_back(s.substr(pos));
    //     break;
    //   }
    // }
    // return ret;
  }

  /**
   * \}
   */
}; // end of namespace turi

#endif


#include "include/core/Ellipsoid.h"

#include "src/Polygon/Polygon.hpp"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>

namespace g2o
{
    ellipsoid::ellipsoid()
    {
    }

    // xyz roll pitch yaw half_scale
    void ellipsoid::fromMinimalVector(const Vector9d& v){
        Eigen::Quaterniond posequat = zyx_euler_to_quat(v(3),v(4),v(5));
        pose = SE3Quat(posequat, v.head<3>());
        scale = v.tail<3>();

        vec_minimal = v;
    }

    // xyz quaternion, half_scale
    void ellipsoid::fromVector(const Vector10d& v){
        pose.fromVector(v.head<7>());
        scale = v.tail<3>();
        vec_minimal = toMinimalVector();
    }

    const Vector3d& ellipsoid::translation() const {return pose.translation();}
    void ellipsoid::setTranslation(const Vector3d& t_) {pose.setTranslation(t_);}
    void ellipsoid::setRotation(const Quaterniond& r_) {pose.setRotation(r_);}
    void ellipsoid::setRotation(const Matrix3d& R) {pose.setRotation(Quaterniond(R));}
    void ellipsoid::setScale(const Vector3d &scale_) {scale=scale_;}

    // apply update to current ellipsoid. exponential map
    ellipsoid ellipsoid::exp_update(const Vector9d& update)
    {
        ellipsoid res;
        res.pose = this->pose*SE3Quat::exp(update.head<6>());
        res.scale = this->scale + update.tail<3>();

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();
        return res;
    }

    // TOBE DELETED.
    ellipsoid ellipsoid::exp_update_XYZABC(const Vector6d& update)
    {
        ellipsoid res;

        Vector6d pose_vec; pose_vec << 0, 0, 0, update[0], update[1], update[2];
        res.pose = this->pose*SE3Quat::exp(pose_vec);
        res.scale = this->scale + update.tail<3>();

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();
        return res;
    }

    Vector9d ellipsoid::ellipsoid_log_error_9dof(const ellipsoid& newone) const
    {
        Vector9d res;
        SE3Quat pose_diff = newone.pose.inverse()*this->pose;

        res.head<6>() = pose_diff.log();
        res.tail<3>() = this->scale - newone.scale;
        return res;
    }

    // change front face by rotate along current body z axis.
    // another way of representing cuboid. representing same cuboid (IOU always 1)
    ellipsoid ellipsoid::rotate_ellipsoid(double yaw_angle) const // to deal with different front surface of cuboids
    {
        ellipsoid res;
        SE3Quat rot(Eigen::Quaterniond(cos(yaw_angle*0.5),0,0,sin(yaw_angle*0.5)),Vector3d(0,0,0));   // change yaw to rotation.
        res.pose = this->pose*rot;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        const double eps = 1e-6;
        if ( (std::abs(yaw_angle-M_PI/2.0) < eps) || (std::abs(yaw_angle+M_PI/2.0) < eps) || (std::abs(yaw_angle-3*M_PI/2.0) < eps))
            std::swap(res.scale(0),res.scale(1));

        return res;
    }

    Vector9d ellipsoid::min_log_error_9dof(const ellipsoid& newone, bool print_details) const
    {
        bool whether_rotate_ellipsoid=true;  // whether rotate cube to find smallest error
        if (!whether_rotate_ellipsoid)
            return ellipsoid_log_error_9dof(newone);

        // NOTE rotating ellipsoid... since we cannot determine the front face consistenly, different front faces indicate different yaw, scale representation.
        // need to rotate all 360 degrees (global cube might be quite different from local cube)
        // this requires the sequential object insertion. In this case, object yaw practically should not change much. If we observe a jump, we can use code
        // here to adjust the yaw.
        Vector4d rotate_errors_norm; Vector4d rotate_angles(-1,0,1,2); // rotate -90 0 90 180
        Eigen::Matrix<double, 9, 4> rotate_errors;
        for (int i=0;i<rotate_errors_norm.rows();i++)
        {
            ellipsoid rotated_cuboid = newone.rotate_ellipsoid(rotate_angles(i)*M_PI/2.0);  // rotate new cuboids
            Vector9d cuboid_error = this->ellipsoid_log_error_9dof(rotated_cuboid);
            rotate_errors_norm(i) = cuboid_error.norm();
            rotate_errors.col(i) = cuboid_error;
        }
        int min_label;
        rotate_errors_norm.minCoeff(&min_label);
        if (print_details)
            if (min_label!=1)
                std::cout<<"Rotate ellipsoid   "<<min_label<<std::endl;
        return rotate_errors.col(min_label);
    }

    // transform a local cuboid to global cuboid  Twc is camera pose. from camera to world
    ellipsoid ellipsoid::transform_from(const SE3Quat& Twc) const{
        ellipsoid res;
        res.pose = Twc*this->pose;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        return res;
    }

    // transform a global cuboid to local cuboid  Twc is camera pose. from camera to world
    ellipsoid ellipsoid::transform_to(const SE3Quat& Twc) const{
        ellipsoid res;
        res.pose = Twc.inverse()*this->pose;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        return res;
    }

    // xyz roll pitch yaw half_scale
    Vector9d ellipsoid::toMinimalVector() const{
        Vector9d v;
        v.head<6>() = pose.toXYZPRYVector();
        v.tail<3>() = scale;
        return v;
    }

    // xyz quaternion, half_scale
    Vector10d ellipsoid::toVector() const{
        Vector10d v;
        v.head<7>() = pose.toVector();
        v.tail<3>() = scale;
        return v;
    }

    Matrix4d ellipsoid::similarityTransform() const
    {
        Matrix4d res = pose.to_homogeneous_matrix();    // 4x4 transform matrix
        Matrix3d scale_mat = scale.asDiagonal();
        res.topLeftCorner<3,3>() = res.topLeftCorner<3,3>()*scale_mat;
        return res;
    }

    void ellipsoid::UpdateValueFrom(const g2o::ellipsoid& e){
        this->miLabel = e.miLabel;
        this->mbColor = e.mbColor;
        this->mvColor = e.mvColor;
        this->miInstanceID = e.miInstanceID;

        this->prob = e.prob;
    }

    ellipsoid::ellipsoid(const g2o::ellipsoid &e) {
        pose = e.pose;
        scale = e.scale;
        vec_minimal = e.vec_minimal;

        UpdateValueFrom(e);
    }

    const ellipsoid& ellipsoid::operator=(const g2o::ellipsoid &e) {
        pose = e.pose;
        scale = e.scale;
        vec_minimal = e.vec_minimal;

        UpdateValueFrom(e);
        return e;
    }

    // ************* Functions As Ellipsoids ***************
    Vector2d ellipsoid::projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib)
    {
        Matrix3Xd  P = generateProjectionMatrix(campose_cw, Kalib);

        Vector3d center_pos = pose.translation();
        Vector4d center_homo = real_to_homo_coord<double>(center_pos);
        Vector3d u_homo = P * center_homo;
        Vector2d u = homo_to_real_coord_vec<double>(u_homo);

        return u;
    }

    // project the ellipsoid into the image plane, and get an ellipse represented by a Vector5d.
    // Ellipse: x_c, y_c, theta, axis1, axis2
    Vector5d ellipsoid::projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix4d Q_star = generateQuadric();
        Matrix3Xd  P = generateProjectionMatrix(campose_cw, Kalib);
        Matrix3d C_star = P * Q_star * P.transpose();
        Matrix3d C = C_star.inverse();
        C = C / C(2,2); // normalize

        SelfAdjointEigenSolver<Matrix3d> es(C);    // ascending sort by default
        VectorXd eigens = es.eigenvalues();

        // If it is an ellipse, the sign of eigen values must be :  1 1 -1
        // Ref book : Multiple View Geometry in Computer Vision
        int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0);
        int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0);

        // matrix to equation coefficients: ax^2+bxy+cy^2+dx+ey+f=0
        double a = C(0,0);
        double b = C(0,1)*2;
        double c = C(1,1);
        double d = C(0,2)*2;
        double e = C(2,1)*2;
        double f = C(2,2);

        // get x_c, y_c, theta, axis1, axis2 from coefficients
        double delta = c*c - 4.0*a*b;
        double k = (a*f-e*e/4.0) - pow((2*a*e-c*d),2)/(4*(4*a*b-c*c));
        double theta = 1/2.0*atan2(b,(a-c));
        double x_c = (b*e-2*c*d)/(4*a*c-b*b);
        double y_c = (b*d-2*a*e)/(4*a*c-b*b);
        double a_2 =  2*(a* x_c*x_c+ c * y_c*y_c+ b *x_c*y_c -1) /(a + c + sqrt((a-c)*(a-c)+b*b));
        double b_2 =  2*(a*x_c*x_c+c*y_c*y_c+b*x_c*y_c -1) /( a + c - sqrt((a-c)*(a-c)+b*b));

        double axis1= sqrt(a_2);
        double axis2= sqrt(b_2);

        Vector5d output;
        output << x_c, y_c, theta, axis1, axis2;

        return output;
    }

    // Get the bounding box from ellipse in image plane
    Vector4d ellipsoid::getBoundingBoxFromEllipse(Vector5d &ellipse) const
    {
        double a = ellipse[3];
        double b = ellipse[4];
        double theta = ellipse[2];
        double x = ellipse[0];
        double y = ellipse[1];

        double cos_theta_2 = cos(theta)*cos(theta);
        double sin_theta_2 = 1- cos_theta_2;

        double x_limit = sqrt(a*a*cos_theta_2+b*b*sin_theta_2);
        double y_limit = sqrt(a*a*sin_theta_2+b*b*cos_theta_2);

        Vector4d output;
        output[0] = x-x_limit; // left up
        output[1] = y-y_limit;
        output[2] = x+x_limit; // right down
        output[3] = y+y_limit;

        return output;
    }

    // Get projection matrix P = K [ R | t ]
    Matrix3Xd ellipsoid::generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix3Xd identity_lefttop;
        identity_lefttop.resize(3, 4);
        identity_lefttop.col(3)=Vector3d(0,0,0);
        identity_lefttop.topLeftCorner<3,3>() = Matrix3d::Identity(3,3);

        Matrix3Xd proj_mat = Kalib * identity_lefttop;
        proj_mat = proj_mat * campose_cw.to_homogeneous_matrix();

        return proj_mat;
    }

    // Get Q^*
    Matrix4d ellipsoid::generateQuadric() const
    {
        Vector4d axisVec;
        axisVec << 1/(scale[0]*scale[0]), 1/(scale[1]*scale[1]), 1/(scale[2]*scale[2]), -1;
        Matrix4d Q_c = axisVec.asDiagonal();
        Matrix4d Q_c_star = Q_c.inverse();
        Matrix4d Q_pose_matrix = pose.to_homogeneous_matrix();   // Twm  model in world,  world to model
        Matrix4d Q_c_star_trans = Q_pose_matrix * Q_c_star * Q_pose_matrix.transpose();

        return Q_c_star_trans;
    }

    // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix.
    Vector4d ellipsoid::getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Vector5d ellipse = projectOntoImageEllipse(campose_cw, Kalib);
        return getBoundingBoxFromEllipse(ellipse);
    }

    Vector3d ellipsoid::getColor(){
        return mvColor.head(3);
    }

    Vector4d ellipsoid::getColorWithAlpha(){
        return mvColor;
    }

    void ellipsoid::setColor(const Vector3d &color_, double alpha){
        mbColor = true;
        mvColor.head<3>() = color_;
        mvColor[3] = alpha;

    }

    bool ellipsoid::isColorSet(){
        return mbColor;
    }

    bool ellipsoid::CheckObservability(const SE3Quat& campose_cw)
    {
        Vector3d ellipsoid_center = toMinimalVector().head(3);    // Pwo
        Vector4d center_homo = real_to_homo_coord_vec<double>(ellipsoid_center);

        Eigen::Matrix4d projMat = campose_cw.to_homogeneous_matrix(); // Tcw
        Vector4d center_inCameraAxis_homo = projMat * center_homo;   // Pco =  Tcw * Pwo
        Vector3d center_inCameraAxis = homo_to_real_coord_vec<double>(center_inCameraAxis_homo);

        if( center_inCameraAxis_homo(2) < 0)    // if the center is behind the camera ; z<0
        {
            return false;
        }
        else
            return true;
    }

    // calculate the IoU Error between two axis-aligned ellipsoid
    double ellipsoid::calculateMIoU(const g2o::ellipsoid& e) const
    {
        return calculateIntersectionError(*this, e);
    }

    double ellipsoid::calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose;
        double z1 = 0; double z2 = pose_diff.translation()[2];

        bool flag_oneBigger = false;
        if( z1 > z2 )
            flag_oneBigger = true;

        double length;
        if( flag_oneBigger )
        {
            length = (z2 + e2.scale[2]) - (z1 - e1.scale[2]);
        }
        else
            length = (z1 + e1.scale[2]) - (z2 - e2.scale[2]);

        if( length < 0 )
            length = 0;     // if they are not intersected

        return length;
    }

    double ellipsoid::calculateArea(const g2o::ellipsoid& e) const
    {
        return e.scale[0]*e.scale[1]*e.scale[2]*8;
    }

    void OutputPolygon(EllipsoidSLAM::Polygon& polygon, double resolution)
    {
        int num = polygon.n;
        for( int i=0;i<num;i++)
            std::cout << i << ":" << polygon[i].x << ", " << polygon[i].y << std::endl;
        std::cout << std::endl;
    }

    // Calculate the intersection area after projected the external cubes of two axis-aligned ellipsoids into XY-Plane.
    double ellipsoid::calculateIntersectionOnXY(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        // First, get the axis-aligned pose error
        g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose;
        double x_center1 = 0; double y_center1 = 0;

        double x_center2 = pose_diff.translation()[0];
        double y_center2 = pose_diff.translation()[1];

        double roll,pitch,yaw;
        quat_to_euler_zyx(pose_diff.rotation(),roll,pitch,yaw);

        double a1 = std::abs(e1.scale[0]);
        double b1 = std::abs(e1.scale[1]);

        double a2 = std::abs(e2.scale[0]);
        double b2 = std::abs(e2.scale[1]);

        // Use polygon to calculate the intersection
        EllipsoidSLAM::Polygon polygon1, polygon2;
        double resolution = 0.001;  // m / resolution = pixel
        polygon1.add(cv::Point(a1/resolution, b1/resolution));    // cvPoint only accepts integer, so use resolution to map meter to pixel ( 0.01 resolution means: 1pixel = 0.01m )
        polygon1.add(cv::Point(-a1/resolution, b1/resolution));
        polygon1.add(cv::Point(-a1/resolution, -b1/resolution));
        polygon1.add(cv::Point(a1/resolution, -b1/resolution));

        double c_length = sqrt(a2*a2+b2*b2);

        double init_theta = CV_PI/2.0 - atan2(a2,b2);
        Vector4d angle_plus_vec;
        angle_plus_vec << 0, atan2(a2,b2)*2, CV_PI, CV_PI+atan2(a2,b2)*2;
        for( int n=0;n<4;n++){
            double angle_plus = angle_plus_vec[n];  // rotate 90deg for four times
            double point_x = c_length * cos( init_theta - yaw + angle_plus ) + x_center2;
            double point_y = c_length * sin( init_theta - yaw + angle_plus ) + y_center2;
            polygon2.add(cv::Point(point_x/resolution, point_y/resolution));
        }

        // calculate the intersection
        EllipsoidSLAM::Polygon interPolygon;
        EllipsoidSLAM::intersectPolygon(polygon1, polygon2, interPolygon);

        // eliminate resolution.
        double inter_area = interPolygon.area();
        double inter_area_in_m = inter_area * resolution * resolution;

        return inter_area_in_m;
    }

    double ellipsoid::calculateIntersectionError(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        //          AXB
        // IoU = ----------
        //          AUB
        //   AXB  =  intersection
        //   AUB  =  A+B-intersection

        // Error of IoU : 1 - IoU
        double areaA = std::abs(calculateArea(e1));
        std::cout << "areaA : " << areaA << std::endl;

        double areaB = std::abs(calculateArea(e2));
        std::cout << "areaB : " << areaB << std::endl;

        double proj_inter = calculateIntersectionOnXY(e1,e2);
        double z_inter = calculateIntersectionOnZ(e1,e2);
        std::cout << "projInter : " << proj_inter << std::endl;
        std::cout << "z_inter : " << z_inter << std::endl;

        double areaIntersection = proj_inter * z_inter;
        std::cout << "areaIntersection : " << areaIntersection << std::endl;

        double MIoU = 1 - ((areaIntersection) / (areaA + areaB - areaIntersection));
        std::cout << "MIoU : " << MIoU << std::endl;
        std::cout << "e1 : " << e1.toMinimalVector().transpose() << std::endl;
        std::cout << "e2 : " << e2.toMinimalVector().transpose() << std::endl;

        return MIoU;
    }

    // ***************** Functions as Cubes ******************

    // calculate the external cube of the ellipsoid
    // 8 corners 3*8 matrix, each row is x y z
    Matrix3Xd ellipsoid::compute3D_BoxCorner() const
    {
        Matrix3Xd corners_body;corners_body.resize(3,8);
        corners_body<< 1, 1, -1, -1, 1, 1, -1, -1,
                1, -1, -1, 1, 1, -1, -1, 1,
                -1, -1, -1, -1, 1, 1, 1, 1;
        Matrix3Xd corners_world = homo_to_real_coord<double>(similarityTransform()*real_to_homo_coord<double>(corners_body));
        return corners_world;
    }

    Matrix2Xd ellipsoid::projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix3Xd corners_3d_world = compute3D_BoxCorner();
        Matrix2Xd corner_2d = homo_to_real_coord<double>(Kalib*homo_to_real_coord<double>(campose_cw.to_homogeneous_matrix()*real_to_homo_coord<double>(corners_3d_world)));

        return corner_2d;
    }

    // get rectangles after projection  [topleft, bottomright]
    Vector4d ellipsoid::projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix2Xd corner_2d = projectOntoImageBoxCorner(campose_cw, Kalib);
        Vector2d bottomright = corner_2d.rowwise().maxCoeff(); // x y
        Vector2d topleft = corner_2d.rowwise().minCoeff();
        return Vector4d(topleft(0),topleft(1),bottomright(0),bottomright(1));
    }

    // get rectangles after projection  [center, width, height]
    Vector4d ellipsoid::projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Vector4d rect_project = projectOntoImageRect(campose_cw, Kalib);  // top_left, bottom_right  x1 y1 x2 y2
        Vector2d rect_center = (rect_project.tail<2>()+rect_project.head<2>())/2;
        Vector2d widthheight = rect_project.tail<2>()-rect_project.head<2>();
        return Vector4d(rect_center(0),rect_center(1),widthheight(0),widthheight(1));
    }

} // g2o


#pragma once

#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
#include <vector>

// ------------------------------------------------------------
#include "aux/hash_specializations.hpp"
#include "laguerren_impl.hpp"

namespace boltzmann {

template <typename NUMERIC>
class LaguerreN
{
 public:
  typedef NUMERIC numeric_t;

 public:
  LaguerreN(int K)
      : Y_(K + 1)
      , K_(K)
      , is_initialized_(false)
  {
  }

  void compute(const std::vector<numeric_t> &x);
  void compute(const numeric_t *x, unsigned int n);

  unsigned int get_npoints() const { return Y_[0].shape()[1]; }

 public:
  typedef boost::multi_array<numeric_t, 2> array_t;

 public:
  const NUMERIC *get(unsigned int k, unsigned int alpha) const;
  void info() const;

 private:
  std::vector<array_t> Y_;
  unsigned int K_;
  bool is_initialized_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::compute(const std::vector<numeric_t> &x)
{
  compute(x.data(), x.size());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::compute(const numeric_t *x, unsigned int n)
{
  // L_n-1
  std::vector<numeric_t> Lnm1(n);
  // L_n-2
  std::vector<numeric_t> Lnm2(n);

  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    Y_[alpha].resize(boost::extents[K_ / 2 + 1][n]);
    // init
    for (size_t xi = 0; xi < n; ++xi) {
      Y_[alpha][0][xi] = boost::math::laguerren(0, alpha, x[xi]);
      Y_[alpha][1][xi] = boost::math::laguerren(1, alpha, x[xi]);
    }

    for (unsigned int k = 2; k <= K_ / 2; ++k) {
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][k][xi] = boost::math::laguerren_next(
            k - 1, alpha, x[xi], Y_[alpha][k - 1][xi], Y_[alpha][k - 2][xi]);
      }
    }
  }
  is_initialized_ = true;
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
const NUMERIC *
LaguerreN<NUMERIC>::get(unsigned int k, unsigned int alpha) const
{
  assert(is_initialized_);
  assert(alpha < Y_.size());
  assert(k < Y_[alpha].shape()[0]);
  return Y_[alpha][k].origin();
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::info() const
{
  unsigned long long int nentries = 0;
  for (unsigned int alpha = 0; alpha < Y_.size(); ++alpha) {
    nentries += Y_[alpha].shape()[0] * Y_[alpha].shape()[1];
  }

  std::cout << " LaguerreN uses " << nentries * sizeof(NUMERIC) / 1e6 << " MB" << std::endl;
}

}  // end namespace boltzmann


/*
 * ex5.cpp
 *
 * 	\brief     Fifth exercixe
 *  \details   This class reads graph-data. It computes the longest shortest path for starting point 1.
 *  \author    Julia Baumbach
 *  \date      03.06.2017
 */

#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/timer/timer.hpp>
#include "dijkstra.h"

using namespace std;

/**
 * \param SOURCE_FILE_PATH default file path if none is hand over while starting the programm
 */
const char* SOURCE_FILE_PATH = "testgraph.gph";

/**
 *\typedef defines a graph by undirected adjacency-list with weighted edges
 */
using graph =  boost::adjacency_list<boost::listS, boost::vecS,
	boost::undirectedS, boost::no_property,
	boost::property<boost::edge_weight_t, int>>;
/*
 * \typedef short version for vertex descriptor from boost::graph_traits
 */
using vertex_descriptor = boost::graph_traits < graph >::vertex_descriptor;

/*
 * Main function. Reads some graph data from a given file and computes the longest
 * shortest path to vertex with number 1.
 */
int main(int argc, char* argv[]){
	//Initialize timer for time measurement
	boost::timer::cpu_timer timer;

	if(argc != 3){
		cerr << "Invalid method call. Please call with ./ex5 -m1/-m2 FILENAME" << endl;
		return EXIT_FAILURE;
	}

	string method = argv[1];
	bool boostMethod = false;
	if (method.compare("-m1") == 0){
		cout << "Using dijkstra from boost-library" << endl;
		boostMethod = true;
	} else if (method.compare("-m2") == 0){
		cout << "Using my dijkstra" << endl;
	} else {
		cerr << "Invalid argument. Please type \"-m1\" for boost-dijkstra or \"-m2\" for manual dijkstra." << endl;
	}

	ifstream infile;
	infile.open(argv[2], ios::in);
	if (!infile){
		cout << "File could not be opened." << endl;
		return 1;
	}

	string line;
	stringstream s;
	unsigned int numberVertices;
	unsigned int numberEdges;

	//Read first line
	if (getline(infile, line)){
		s.str(line);
		s >> numberVertices >> numberEdges;
		//Number of vertices is one more than the real number of vertices for storing all the vertices correctly (it's 1-based)
		numberVertices++;
	} else {
		cerr << "Empty file. Exit program" << endl;
		return 1;
	}

	//Reading the edge data
	Edges edges;
	WeightMap weights;
	int startEdge;
	int endEdge;
	int weight;

	while (getline(infile, line)){
		if (!line.empty()){
			s.clear();
			s.str(line);
			s >> startEdge >> endEdge >> weight;
			edges.push_back(make_pair(startEdge, endEdge));
			weights.push_back(weight);
		}
	}
	infile.close();

	WeightMap weightMap(numberVertices);

	if (boostMethod){
		//Creating a graph g
		graph g{edges.begin(), edges.end(), weights.begin(), numberVertices};

		//storing the shortest paths and its weights
		vector<vertex_descriptor> directions(numberVertices);

		boost::dijkstra_shortest_paths(g, 1,//
				boost::predecessor_map(//
						boost::make_iterator_property_map(directions.begin(), get(boost::vertex_index, g)))//
				.distance_map(//
						boost::make_iterator_property_map(weightMap.begin(), get(boost::vertex_index, g))));
	}
	else {
		dijkstra myDijkstra(weights, edges, numberVertices);
		weightMap = myDijkstra.computeShortestPath(1);
	}

	//Compute the longest shortest path
	int weightOfLongestShortestPath = -1;
	int indexOfVertex = -1;
	int totalWeight;
	for(unsigned int i = 2; i < numberVertices; i++){
		totalWeight = weightMap[i];
		if (totalWeight > weightOfLongestShortestPath){
			weightOfLongestShortestPath = totalWeight;
			indexOfVertex = i;
		}
	}

	cout << "RESULT VERTEX " << indexOfVertex << endl;
	cout << "RESULT DIST " << weightOfLongestShortestPath << endl;

	//Print measured time
	boost::timer::cpu_times times = timer.elapsed();
	cout << "Wall-clock time: " << times.wall * 1e-9 << " seconds" << endl;
	cout << "User-time: " << times.user * 1e-9 <<  " seconds" << endl;

	return 0;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>  // have to include Eigen/Core before #include <opencv2/core/eigen.hpp> !!
#include <opencv2/core/eigen.hpp>

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace Eigen;
using namespace std;
using namespace cv;

void feature_matching(const Mat& img1, const Mat& img2,
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches)
    {
        Mat descriptors_1, descriptors_2;
        Ptr<FeatureDetector> detector = ORB::create();
        Ptr<DescriptorExtractor> descriptor = ORB::create();

        // compute FAST corners
        detector->detect(img1, keypoints1);
        detector->detect(img2, keypoints2);

        // compute BRIEF descriptor
        descriptor->compute(img1, keypoints1, descriptors_1);
        descriptor->compute(img2, keypoints2, descriptors_2);

        Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(4);
        /*
        FLANNBASED = 1,
        BRUTEFORCE = 2,
        BRUTEFORCE_L1 = 3,
        BRUTEFORCE_HAMMING = 4,
        BRUTEFORCE_HAMMINGLUT = 5,
        BRUTEFORCE_SL2 = 6 */

        vector<DMatch> allmatches;
        matcher->match(descriptors_1, descriptors_2, allmatches);

        // Filter all match points
        // Find maximum and minimum distance
        double min_dist = 1000000, max_dist = 0;
        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            double dist = allmatches[i].distance;
            if ( dist < min_dist ) min_dist = dist;
            if ( dist > max_dist ) max_dist = dist;
        }

        cout << "max distance: " << max_dist << endl;
        cout << "min distance: " << min_dist << endl;

        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            if ( allmatches[i].distance <= max ( 2*min_dist, 30.0 ) )
            {
                matches.push_back ( allmatches[i] );
            }
        }
    }

void pose_estimation_2d2d(
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches,
    Mat &R, Mat&t){

        Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

        vector<Point2f> points1;
        vector<Point2f> points2;

        for ( int i = 0; i < ( int ) matches.size(); i++ )
        {
            points1.push_back ( keypoints1[matches[i].queryIdx].pt );
            points2.push_back ( keypoints2[matches[i].trainIdx].pt );
        }

        Mat fundamental_matrix;
        fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
        cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

        // Essential matrix
        Point2d principal_point ( 325.1, 249.7 );
        double focal_length = 521;
        Mat essential_matrix;
        essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
        cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

        // Homography Matrix
        Mat homography_matrix;
        homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
        cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

        // Calculate R and t
        recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
        cout<<"R is "<<endl<<R<<endl;
        cout<<"t is "<<endl<<t<<endl;
    }

int main(int argc, char** argv){
    Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> matches;
    feature_matching(img1, img2, keypoints1, keypoints2, matches);

    cout<< "Total " << matches.size() << "matched feature points" << endl;

    // pose estimation for two images
    Mat R, t;  // Rotation and translation
    pose_estimation_2d2d(keypoints1, keypoints2, matches, R, t);

    // epipolar geometry
    Mat K = ( Mat_<double> (3, 3) << 529.0, 0, 325.1, 0, 521.0, 249.1, 0, 0, 1);
    Mat t_x = ( Mat_<double> ( 3,3 ) <<
                0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
                t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
                -t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );

    for(DMatch mm : matches){
        Point2d pt1 = Point2d
           (
               ( keypoints1[ mm.queryIdx ].pt.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( keypoints1[ mm.queryIdx ].pt.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
        Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );

        Point2d pt2 = Point2d
           (
               ( keypoints2[ mm.trainIdx ].pt.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( keypoints2[ mm.trainIdx ].pt.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
        Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }

    Mat E = t_x * R;
    cout << E.size() << endl;
    Eigen::Matrix<float, 3, 3> b;
    cv2eigen(E, b);
    cout << "Essential Matrix Eigenvalues and Eigenvectors" << endl;
    cout << b.eigenvalues().col(0)[0] << endl;
    cout << b.eigenvalues().col(0)[1] << endl;
    cout << b.eigenvalues().col(0)[2] << endl;
    // cout << b.eigenvectors() << endl;
    return 0;
}


/*
 * H1H1.cpp
 *
 *  Created on: 13.03.2018
 *      Author: thies
 */

#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>
#include <deal.II/lac/vector.h>
#include <norms/H1H1.h>
#include <stddef.h>
#include <cmath>
#include <iostream>
#include <vector>

namespace wavepi {
namespace norms {

using namespace dealii;

inline double square(const double x) { return x * x; }
inline double pow4(const double x) { return x * x * x * x; }

template <int dim>
H1H1<dim>::H1H1(double alpha, double gamma) : alpha_(alpha), gamma_(gamma) {}

template <int dim>
double H1H1<dim>::norm(const DiscretizedFunction<dim>& u) const {
  auto mesh = u.get_mesh();

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv = u.calculate_derivative();

  double result = 0;

  for (size_t i = 0; i < mesh->length(); i++) {
    double nrm2 = mesh->get_mass_matrix(i)->matrix_norm_square(u[i]) +
                  gamma_ * mesh->get_laplace_matrix(i)->matrix_norm_square(u[i]);
    double nrm2_deriv = mesh->get_mass_matrix(i)->matrix_norm_square(deriv[i]) +
                        gamma_ * mesh->get_laplace_matrix(i)->matrix_norm_square(deriv[i]);

    // + trapezoidal rule in time:
    if (i > 0) result += (nrm2 + alpha_ * nrm2_deriv) / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (nrm2 + alpha_ * nrm2_deriv) / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return std::sqrt(result);
}

template <int dim>
double H1H1<dim>::dot(const DiscretizedFunction<dim>& u, const DiscretizedFunction<dim>& v) const {
  auto mesh     = u.get_mesh();
  double result = 0.0;

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv  = u.calculate_derivative();
  auto Vderiv = v.calculate_derivative();

  for (size_t i = 0; i < mesh->length(); i++) {
    double doti = mesh->get_mass_matrix(i)->matrix_scalar_product(u[i], v[i]) +
                  gamma_ * mesh->get_laplace_matrix(i)->matrix_scalar_product(u[i], v[i]);
    double doti_deriv = mesh->get_mass_matrix(i)->matrix_scalar_product(deriv[i], Vderiv[i]) +
                        gamma_ * mesh->get_laplace_matrix(i)->matrix_scalar_product(deriv[i], Vderiv[i]);

    // + trapezoidal rule in time
    if (i > 0) result += (doti + alpha_ * doti_deriv) / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (doti + alpha_ * doti_deriv) / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return result;
}

template <int dim>
void H1H1<dim>::dot_transform(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  // X = (T + \alpha_ D^t T D) * (M+gamma_ L),
  // M = blocks of mass matrices, D = derivative, T = trapezoidal rule, L = blocks of laplace matrices

  DiscretizedFunction<dim> tmp(mesh, u.get_norm());
  for (size_t i = 0; i < mesh->length(); i++)
    mesh->get_laplace_matrix(i)->vmult(tmp[i], u[i]);

  u.mult_mass();
  u.add(gamma_, tmp);

  auto dx = u.calculate_derivative();

  // trapezoidal rule
  // (has to happen between D and D^t for dx)
  for (size_t i = 0; i < mesh->length(); i++) {
    double factor = 0.0;

    if (i > 0) factor += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;
    if (i < mesh->length() - 1) factor += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;

    dx[i] *= factor;
    u[i] *= factor;
  }

  auto dtdx = dx.calculate_derivative_transpose();

  // add derivative term
  for (size_t i = 0; i < mesh->length(); i++) {
    u[i].add(alpha_, dtdx[i]);
  }
}

template <int dim>
void H1H1<dim>::dot_transform_inverse(DiscretizedFunction<dim>& u) {
  LogStream::Prefix p("h1h1_transform");

  auto mesh = u.get_mesh();
  AssertThrow(mesh->length() > 7, ExcInternalError());

  Timer timer;
  timer.start();

  // space part: solve mass+ɣ*Δ
  SparseMatrix<double> system_matrix(*mesh->get_sparsity_pattern(0));
  system_matrix.copy_from(*mesh->get_mass_matrix(0));
  system_matrix.add(gamma_, *mesh->get_laplace_matrix(0));
  Vector<double> sp_tmp(u[0].size());

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(u[0].size() == u[i].size(), ExcInternalError());
    LogStream::Prefix p("step-" + Utilities::int_to_string(i, 4));

    SolverControl solver_control(2000, 1e-10 * u[i].l2_norm());
    SolverCG<> cg(solver_control);
    PreconditionIdentity precondition = PreconditionIdentity();

    sp_tmp = 0.0;
    cg.solve(system_matrix, sp_tmp, u[i], precondition);
    u[i] = sp_tmp;
  }

  // time part
  if (umfpack.n() != mesh->length()) factorize_matrix(mesh);

  // just to be sure
  for (size_t i = 0; i < mesh->length(); i++)
    Assert(u[i].size() == u[0].size(), ExcInternalError());

  // solve for every DoF
  Vector<double> tmp(mesh->length());

  for (size_t i = 0; i < u[0].size(); i++) {
    for (size_t j = 0; j < mesh->length(); j++)
      tmp[j] = u[j][i];

    umfpack.solve(tmp);

    for (size_t j = 0; j < mesh->length(); j++)
      u[j][i] = tmp[j];
  }

  deallog << "solved in " << Util::format_duration(timer.wall_time()) << std::endl;
}

template <int dim>
void H1H1<dim>::factorize_matrix(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  deallog << "factorizing matrix" << std::endl;

  SparsityPattern pattern(mesh->length(), mesh->length(), 3);

  pattern.add(0, 0);
  pattern.add(0, 1);
  pattern.add(1, 0);
  pattern.add(1, 1);

  for (size_t i = 2; i < mesh->length() - 2; i++) {
    // fill row i and column i

    pattern.add(i, i);

    pattern.add(i, i - 2);
    pattern.add(i - 2, i);

    pattern.add(i, i + 2);
    pattern.add(i + 2, i);
  }

  pattern.add(mesh->length() - 2, mesh->length() - 1);
  pattern.add(mesh->length() - 2, mesh->length() - 1);
  pattern.add(mesh->length() - 1, mesh->length() - 2);
  pattern.add(mesh->length() - 1, mesh->length() - 1);

  pattern.compress();

  // coefficients of trapezoidal rule
  std::vector<double> lambdas(mesh->length(), 0.0);

  for (size_t i = 0; i < mesh->length(); i++) {
    if (i > 0) lambdas[i] += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;

    if (i < mesh->length() - 1) lambdas[i] += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;
  }

  SparseMatrix<double> matrix(pattern);

  double sq20 = 1.0 / square(mesh->get_time(2) - mesh->get_time(0));
  double sq10 = 1.0 / square(mesh->get_time(1) - mesh->get_time(0));
  double sq31 = 1.0 / square(mesh->get_time(3) - mesh->get_time(1));

  matrix.set(0, 0, lambdas[1] * sq20 + lambdas[0] * sq10);
  matrix.set(1, 1, lambdas[2] * sq31 + lambdas[0] * sq10);
  matrix.set(0, 1, -lambdas[0] * sq10);
  matrix.set(1, 0, -lambdas[0] * sq10);

  for (size_t i = 2; i < mesh->length() - 2; i++) {
    // fill row i and column i

    double sq20  = 1.0 / square(mesh->get_time(i + 2) - mesh->get_time(i));
    double sq0m2 = 1.0 / square(mesh->get_time(i) - mesh->get_time(i - 2));

    matrix.set(i, i, lambdas[i + 1] * sq20 + lambdas[i - 1] * sq0m2);

    matrix.set(i, i - 2, -lambdas[i - 1] * sq0m2);
    matrix.set(i - 2, i, -lambdas[i - 1] * sq0m2);

    matrix.set(i, i + 2, -lambdas[i + 1] * sq20);
    matrix.set(i + 2, i, -lambdas[i + 1] * sq20);
  }

  // (symmetric to the first entries)
  size_t N = mesh->length() - 1;  // makes it easier to read

  sq20 = 1.0 / square(mesh->get_time(N - 2) - mesh->get_time(N));
  sq10 = 1.0 / square(mesh->get_time(N - 1) - mesh->get_time(N));
  sq31 = 1.0 / square(mesh->get_time(N - 3) - mesh->get_time(N - 1));

  matrix.set(N, N - 0, lambdas[N - 1] * sq20 + lambdas[N] * sq10);
  matrix.set(N - 1, N - 1, lambdas[N - 2] * sq31 + lambdas[N] * sq10);
  matrix.set(N, N - 1, -lambdas[N] * sq10);
  matrix.set(N - 1, N, -lambdas[N] * sq10);

  matrix *= alpha_;

  // L2 part (+ trapezoidal rule)
  for (size_t i = 0; i < mesh->length(); i++)
    matrix.add(i, i, lambdas[i]);

  umfpack.factorize(matrix);
}

template <int dim>
void H1H1<dim>::dot_solve_mass_and_transform(DiscretizedFunction<dim>& u) {
  // X = (T + \alpha_ D^t T D) * (M+gamma_ L),
  // M = blocks of mass matrices, D = derivative, T = trapezoidal rule, L = blocks of laplace matrices
  u.solve_mass();
  dot_transform(u);
}

template <int dim>
void H1H1<dim>::dot_mult_mass_and_transform_inverse(DiscretizedFunction<dim>& u) {
  u.mult_mass();
  dot_transform_inverse(u);
}

template <int dim>
std::string H1H1<dim>::name() const {
  return "H¹([0,T], H¹(Ω))";
}

template <int dim>
std::string H1H1<dim>::unique_id() const {
  return "H¹([0,T], H¹(Ω)) with ɣ=" + std::to_string(gamma_) + ", α=" + std::to_string(alpha_);
}

template class H1H1<1>;
template class H1H1<2>;
template class H1H1<3>;

} /* namespace norms */
} /* namespace wavepi */


#include "calibrate.hpp"

#include "parse.hpp"

#include <Eigen/SVD>

#include <iostream>
#include <stdexcept>

// Assuming that the photos have been distributed roughly evenly around the
// circle and that the rotation axis was close to the global vertical axis.
// The returned vector is the camera space +y vector in IMU coordinates. y is
// down on the image.
Eigen::Vector3f get_y_axis(std::vector<Eigen::Vector3f> const& photo_gravity) {
	Eigen::Vector3f vertical_axis = Eigen::Vector3f::Zero();

	for (auto const& g : photo_gravity)
		vertical_axis += g;

	return vertical_axis.normalized();
}

// Assumes that the camera x axis was roughly orthogonal to the world vertcal
// axis and the photos were taken while rotating exactly around the camera x
// axis.
Eigen::Vector3f get_x_axis(std::vector<Eigen::Vector3f> const& photo_gravity) {
	// Simple linear regression
	const int n = photo_gravity.size();

	assert(n >= 4);

	Eigen::MatrixXf m(n, 4);
	for (int i = 0; i < n; ++i) {
		m.block<1, 3>(i, 0) = photo_gravity[i].transpose().normalized();
	}

	m.col(3) = Eigen::VectorXf::Ones(n);

	// We find a plane through the data. The first three components are
	// the normal of the plane, the last component is the offset.
	Eigen::Vector4f w = m.bdcSvd(Eigen::ComputeFullV).matrixV().col(3);
	w /= w.head<3>().norm();

	Eigen::Vector3f normal = w.head<3>();

	// Here we make sure that the rotation axis was close to orthogonal
	// to gravity. The offset of the plane is equal the sin of the
	// angle between the rotation axis and a perfectly horizontal line.
	const float sin_angle = w(3);
	assert(std::abs(sin_angle) < 0.2);

	// We check if the camera was rotated in positive or negative
	// direction around `normal` by checking how the first and last
	// vector split up the circle into sectors and seeing into which
	// of the sectors the middle recording falls.

	std::array<Eigen::Vector3f, 3> vecs_on_plane = {
	    m.block<1, 3>(0, 0), m.block<1, 3>(n / 2, 0), m.block<1, 3>(n - 1, 0)};

	for (auto& v : vecs_on_plane)
		v -= v.dot(normal) * normal;

	const Eigen::Vector3f centre_of_positive_sector =
	    normal.cross(vecs_on_plane[0]) - normal.cross(vecs_on_plane[2]);

	if (centre_of_positive_sector.dot(vecs_on_plane[1]) <
	    centre_of_positive_sector.dot(vecs_on_plane[0]))
		normal *= -1;

	return normal;
}

// Returns the rotation from IMU frame to Camera frame.
Eigen::Quaternionf calibrate(std::ifstream& recording_infile,
                             int x_session_index, int y_session_index,
                             int nr_skip_measurements) {
	int session_index = -1;

	bool x_calibrated = false;
	bool y_calibrated = false;

	if (x_session_index == y_session_index) {
		throw std::invalid_argument(
		    "The indices for calibrating the x and y axes can't be equal.");
	}

	std::vector<Eigen::Vector3f> measurements;
	measurements.reserve(30);

	std::vector<Eigen::Vector3f> photo_gravities;

	Eigen::Vector3f x_axis;
	Eigen::Vector3f y_axis;

	const auto run_calibrations = [&]() {
		if (session_index == x_session_index) {
			x_axis = get_x_axis(photo_gravities);
			x_calibrated = true;
		} else if (session_index == y_session_index) {
			y_axis = get_y_axis(photo_gravities);
			y_calibrated = true;
		}
	};

	log_entry_types entry_type;
	while (read_next_entry(recording_infile, entry_type)) {
		switch (entry_type) {
		case log_entry_types::photo_event:
			parse_photo(recording_infile).millis;

			if (session_index >= 0 && (session_index == x_session_index ||
			                           session_index == y_session_index)) {
				Eigen::Vector3f g = Eigen::Vector3f::Zero();
				int nr_relevant_measurements =
				    measurements.size() - nr_skip_measurements;
				for (int i = 0; i < nr_relevant_measurements; ++i)
					g += measurements[i];
				g /= nr_relevant_measurements;

				photo_gravities.push_back(g);
			}

			measurements.clear();
			break;
		case log_entry_types::startup:

			if (session_index >= 0)
				run_calibrations();

			parse_startup(recording_infile);
			photo_gravities.clear();
			measurements.clear();
			++session_index;
			break;
		case log_entry_types::gravity_vector:
			measurements.push_back(parse_gravity(recording_infile).g);
			break;
		}
	}

	if (session_index >= 0)
		run_calibrations();

	// If only one of the axes was calibrated we still want to print the
	// result, this is useful for testing.
	if (x_calibrated)
		std::cout << "Sensor x axis in IMU coordinates: " << x_axis.transpose()
		          << '\n';
	if (y_calibrated)
		std::cout << "Sensor y axis in IMU coordinates: " << y_axis.transpose()
		          << '\n';

	if (x_calibrated && y_calibrated) {
		// running a complete calibration

		Eigen::Matrix3f rotation_matrix;
		rotation_matrix << x_axis, y_axis, x_axis.cross(y_axis);

		// Invert the rotation because we want to get the conversion from IMU
		// frame to camera.
		return Eigen::Quaternionf(rotation_matrix).conjugate();
	} else {
		if (x_session_index >= session_index)
			throw std::runtime_error("Invalid x session index.");
		if (y_session_index >= session_index)
			throw std::runtime_error("Invalid y session index.");
	}

	throw std::runtime_error("Need both x and y for complete calibration.");
}


#include "vds.h"

#include <math.h>
#include <stdio.h>
#include <boost/math/constants/constants.hpp>
#include "vector_td_utilities.h"
#include <boost/range/combine.hpp>
#include <boost/range/algorithm.hpp>
#include "hoNDArray_math.h"

constexpr double GAMMA = 4258.0;        /* Hz/G */
constexpr double PI = boost::math::constants::pi<double>();

/* #define TESTCODE 	For testing as regular C code... */

/*
  %
  %	VARIABLE DENSITY SPIRAL GENERATION:
  %	----------------------------------
  %
  %	This is a general description of how the following C code
  %	works.  This text is taken from a matlab script, vds.m, from
  %	which the C code was derived.  However, note that the C code
  %	runs considerably faster.
  %
  %
  %	Function generates variable density spiral which traces
  %	out the trajectory
  %
  %			k(t) = r(t) exp(i*q(t)), 		[1]
  %
  %	Where q IS THE SAME AS theta, and r IS THE SAME AS kr.
  %
  %		r and q are chosen to satisfy:
  %
  %		1) Maximum gradient amplitudes and slew rates.
  %		2) Maximum gradient due to FOV, where FOV can
  %		   vary with k-space radius r, as
  %
  %			FOV(r) = F0 + F1*r + F2*r*r 		[2]
  %
  %
  %	INPUTS:
  %	-------
  %	smax = maximum slew rate G/cm/s
  %	gmax = maximum gradient G/cm (limited by Gmax or FOV)
  %	T = sampling period (s) for gradient AND acquisition.
  %	N = number of interleaves.
  %	F0,F1,F2 = FOV coefficients with respect to r - see above.
  %	rmax= value of k-space radius at which to stop (cm^-1).
  %		rmax = 1/(2*resolution);
  %
  %
  %	OUTPUTS:
  %	--------
  %	k = k-space trajectory (kx+iky) in cm-1.
  %	g = gradient waveform (Gx+iGy) in G/cm.
  %	s = derivative of g (Sx+iSy) in G/cm/s.
  %	time = time points corresponding to above (s).
  %	r = k-space radius vs time (used to design spiral)
  %	theta = atan2(ky,kx) = k-space angle vs time.
  %
  %
  %	METHODS:
  %	--------
  %	Let r1 and r2 be the first derivatives of r in [1].
  %	Let q1 and q2 be the first derivatives of theta in [1].
  %	Also, r0 = r, and q0 = theta - sometimes both are used.
  %	F = F(r) defined by F0,F1,F2.
  %
  %	Differentiating [1], we can get G = a(r0,r1,q0,q1,F)
  %	and differentiating again, we get S = b(r0,r1,r2,q0,q1,q2,F)
  %
  %	(functions a() and b() are reasonably easy to obtain.)
  %
  %	FOV limits put a constraint between r and q:
  %
  %		dr/dq = N/(2*pi*F)				[3]
  %
  %	We can use [3] and the chain rule to give
  %
  %		q1 = 2*pi*F/N * r1				[4]
  %
  %	and
  %
  %		q2 = 2*pi/N*dF/dr*r1^2 + 2*pi*F/N*r2		[5]
  %
  %
  %
  %	Now using [4] and [5], we can substitute for q1 and q2
  %	in functions a() and b(), giving
  %
  %		G = c(r0,r1,F)
  %	and 	S = d(r0,r1,r2,F,dF/dr)
  %
  %
  %	Using the fact that the spiral should be either limited
  %	by amplitude (Gradient or FOV limit) or slew rate, we can
  %	solve
  %		|c(r0,r1,F)| = |Gmax|  				[6]
  %
  %	analytically for r1, or
  %
  %	  	|d(r0,r1,r2,F,dF/dr)| = |Smax|	 		[7]
  %
  %	analytically for r2.
  %
  %	[7] is a quadratic equation in r2.  The smaller of the
  %	roots is taken, and the real part of the root is used to
  %	avoid possible numeric errors - the roots should be real
  %	always.
  %
  %	The choice of whether or not to use [6] or [7], and the
  %	solving for r2 or r1 is done by calcthetadotdot().
  %
  %	Once the second derivative of theta(q) or r is obtained,
  %	it can be integrated to give q1 and r1, and then integrated
  %	again to give q and r.  The gradient waveforms follow from
  %	q and r.
  %
  %	Brian Hargreaves -- Sept 2000.
  %
  %
*/

namespace Gadgetron {

    /* ----------------------------------------------------------------------- */
    void calcthetadotdot(double slewmax, double gradmax, double kr,
                         double krdot, double Tgsample, double Tdsample, int Ninterleaves,
                         double *fov, int numfov, double *thetadotdot, double *krdotdot)
    /*
     * Function calculates the 2nd derivative of kr and theta at each
     * sample point within calc_vds().  ie, this is the iterative loop
     * for calc_vds.  See the text at the top of this file for more details
     * */

    //double slewmax;		/*	Maximum slew rate, G/cm/s		*/
    //double gradmax;		/* 	maximum gradient amplitude, G/cm	*/
    //double kr;		/* 	Current kr. */
    //double krdot;		/*	Current krdot. */
    //double Tgsample;	/*	Gradient Sample period (s) 	*/
    //double Tdsample;	/*	Data Sample period (s) 		*/
    //int Ninterleaves;	/*	Number of interleaves			*/
    //double *fov;		/*	FOV coefficients		*/
    //int numfov;		/*	Number of FOV coefficients		*/
    //double *thetadotdot;	/*	[output] 2nd derivative of theta.	*/
    //double *krdotdot;	/*	[output] 2nd derivative of kr		*/

    /* ----------------------------------------------------------------------- */
    {
        double fovval = 0;    /* FOV for this value of kr	*/
        double dfovdrval = 0;    /* dFOV/dkr for this value of kr	*/
        double gmaxfov;        /* FOV-limited Gmax.	*/
        double maxkrdot;
        int count;

        double tpf;    /* Used to simplify expressions. */
        double tpfsq;    /* 	" 		"        */

        double qdfA, qdfB, qdfC;    /* Quadratic formula coefficients */
        double rootparta, rootpartb;

        /* Calculate the actual FOV and dFOV/dkr for this R,
         * based on the fact that the FOV is expressed
         * as a polynomial in kr.*/

        for (count = 0; count < numfov; count++) {
            fovval = fovval + fov[count] * pow(kr, count);
            if (count > 0)
                dfovdrval = dfovdrval + count * fov[count] * pow(kr, count - 1);
        }

        /* Calculate FOV limit on gmax.  This is the rate of motion along
         * a trajectory, and really should not be a limitation.  Thus,
         * it is reasonable to comment out the following lines. */

        gmaxfov = 1 / GAMMA / fovval / Tdsample;
        if (gradmax > gmaxfov)
            gradmax = gmaxfov;

        /* Maximum dkr/dt, based on gradient amplitude.  */

        maxkrdot = sqrt(pow(GAMMA * gradmax, 2) / (1 + pow(2 * PI * fovval * kr / Ninterleaves, 2)));

        /* These two are just to simplify expressions below */
        tpf = 2 * PI * fovval / Ninterleaves;
        tpfsq = pow(tpf, 2);

        if (krdot > maxkrdot)    /* Then choose krdotdot so that krdot is in range */
        {
            *krdotdot = (maxkrdot - krdot) / Tgsample;
        } else            /* Choose krdotdot based on max slew rate limit. */
        {

            /* Set up for quadratic formula solution. */

            qdfA = 1 + tpfsq * kr * kr;
            qdfB = 2 * tpfsq * kr * krdot * krdot +
                   2 * tpfsq / fovval * dfovdrval * kr * kr * krdot * krdot;
            qdfC = pow(tpfsq * kr * krdot * krdot, 2) + 4 * tpfsq * pow(krdot, 4) +
                   pow(tpf * dfovdrval / fovval * kr * krdot * krdot, 2) +
                   4 * tpfsq * dfovdrval / fovval * kr * pow(krdot, 4) -
                   pow(GAMMA * slewmax, 2);

            rootparta = -qdfB / (2 * qdfA);
            rootpartb = qdfB * qdfB / (4 * qdfA * qdfA) - qdfC / qdfA;

            if (rootpartb < 0)    /* Safety check - if complex, take real part.*/

                *krdotdot = rootparta;

            else
                *krdotdot = rootparta + sqrt(rootpartb);

            /* Could check resulting slew rate here, as in q2r21.m. */
        }

        /* Calculate thetadotdot */

        *thetadotdot = tpf * dfovdrval / fovval * krdot * krdot + tpf * (*krdotdot);
    }

    /* ----------------------------------------------------------------------- */
    void
    calc_vds(double slewmax, double gradmax, double Tgsample, double Tdsample, int Ninterleaves,
             double *fov, int numfov, double krmax,
             int ngmax, double **xgrad, double **ygrad, int *numgrad)

    /*	Function designs a variable-density spiral gradient waveform
     *	that is defined by a number of interleaves, resolution (or max number
     *	of samples), and field-of-view.
     *	The field-of-view is a polynomial function of the
     *	k-space radius, so fov is an array of coefficients so that
     *
     *	FOV = fov[0]+fov[1]*kr+fov[2]*kr^2+ ... +fov[numfov-1]*kr^(numfov-1)
     *
     * 	Gradient design is subject to a constant-slew-rate-limit model,
     * 	with maximum slew rate slewmax, and maximum gradient amplitude
     * 	of gradmax.
     *
     * 	Tgsample is the gradient sampling rate, and Tdsample is the data
     * 	sampling rate.  It is highly recommended to OVERSAMPLE the gradient
     * 	in the design to make the integration more stable.
     *
     * */

    //double slewmax;		/*	Maximum slew rate, G/cm/s		*/
    //double gradmax;		/* 	maximum gradient amplitude, G/cm	*/
    //double Tgsample;	/*	Gradient Sample period (s)		*/
    //double Tdsample;	/*	Data Sample period (s)			*/
    //int Ninterleaves;	/*	Number of interleaves			*/
    //double *fov;		/*	FOV coefficients		*/
    //int numfov;		/*	Number of FOV coefficients		*/
    //double krmax;		/*	Maximum k-space extent (/cm)		*/
    //int ngmax;		/*	Maximum number of gradient samples	*/
    //double **xgrad;		/* 	[output] X-component of gradient (G/cm) */
    //double **ygrad;		/*	[output] Y-component of gradient (G/cm)	*/
    //int *numgrad;		/* 	[output] Number of gradient samples */

    /* ----------------------------------------------------------------------- */
    {
        int gradcount = 0;

        double kr = 0;            /* Current value of kr	*/
        double krdot = 0;        /* Current value of 1st derivative of kr */
        double krdotdot = 0;        /* Current value of 2nd derivative of kr */

        double theta = 0;            /* Current value of theta */
        double thetadot = 0;        /* Current value of 1st derivative of theta */
        double thetadotdot = 0;        /* Current value of 2nd derivative of theta */

        double lastkx = 0;        /* x-component of last k-location. */
        double lastky = 0;        /* y-component of last k-location */
        double kx, ky;            /* x and y components of current k-location */

        double *gxptr, *gyptr;        /* Pointers to gradient variables. */

        /* First just find the gradient length. */

        while ((kr < krmax) && (gradcount < ngmax)) {
            calcthetadotdot(slewmax, gradmax, kr, krdot, Tgsample, Tdsample,
                            Ninterleaves, fov, numfov, &thetadotdot, &krdotdot);

            /* Integrate to obtain new values of kr, krdot, theta and thetadot:*/

            thetadot = thetadot + thetadotdot * Tgsample;
            theta = theta + thetadot * Tgsample;

            krdot = krdot + krdotdot * Tgsample;
            kr = kr + krdot * Tgsample;

            gradcount++;

        }

        /* Allocate memory for gradients. */

        *numgrad = gradcount;

        //*xgrad = (double *)malloc(*numgrad*sizeof(double));
        //*ygrad = (double *)malloc(*numgrad*sizeof(double));

        *xgrad = new double[*numgrad];
        *ygrad = new double[*numgrad];

        /* Reset parameters */

        kr = 0;
        krdot = 0;
        theta = 0;
        thetadot = 0;
        gradcount = 0;
        gxptr = *xgrad;
        gyptr = *ygrad;

        /* Now re-calculate gradient to find length. */

        while ((kr < krmax) && (gradcount < ngmax)) {
            calcthetadotdot(slewmax, gradmax, kr, krdot, Tgsample, Tdsample,
                            Ninterleaves, fov, numfov, &thetadotdot, &krdotdot);

            /* Integrate to obtain new values of kr, krdot, theta and thetadot:*/

            thetadot = thetadot + thetadotdot * Tgsample;
            theta = theta + thetadot * Tgsample;

            krdot = krdot + krdotdot * Tgsample;
            kr = kr + krdot * Tgsample;

            /* Define current gradient values from kr and theta. */

            kx = kr * cos(theta);
            ky = kr * sin(theta);
            *gxptr++ = (1 / GAMMA / Tgsample) * (kx - lastkx);
            *gyptr++ = (1 / GAMMA / Tgsample) * (ky - lastky);
            lastkx = kx;
            lastky = ky;

            gradcount++;
        }

    }

    /* ----------------------------------------------------------------------- */
    void
    calc_traj(double *xgrad, double *ygrad, int ngrad, int Nints, double Tgsamp, double krmax,
              double **x_trajectory, double **y_trajectory,
              double **weights) //, double** y_weights)
    /*
     *inputs:
     *      xgrad   X gradient waveform
     *      ygrad   Y gradient waveform
     *      ngrad   number of gradient samples
     *      Nints   number of interleaves
     *      Tgsamp  sampling time for gradients
     *
     *outputs:
     *      x_trajectory    X position in k-space
     *      y_trajectory    Y position in k-space
     *      x_weights       X weighting
     *      y_weights       Y weighting
     *
     **/
    {

        *x_trajectory = new double[(ngrad * Nints)];
        *y_trajectory = new double[(ngrad * Nints)];
        *weights = new double[(ngrad * Nints)];

        double *txptr = *x_trajectory;
        double *typtr = *y_trajectory;
        double *wptr = *weights;

        for (int inter = 0; inter < Nints; inter++) {
            double rotation = (inter * 2 * PI) / Nints;
            double x_tr = 0;
            double y_tr = 0;
            float x_temp, y_temp;
            for (int gradcount = 0; gradcount < ngrad; gradcount++) {
                if (gradcount > 0) {
                    x_tr += (GAMMA) * xgrad[gradcount - 1] * Tgsamp;
                    y_tr += (GAMMA) * ygrad[gradcount - 1] * Tgsamp;
                }

                x_temp = (x_tr * cos(rotation)) + (y_tr * sin(rotation));
                y_temp = -(x_tr * sin(rotation)) + (y_tr * cos(rotation));
                *(txptr++) = x_temp / krmax;
                *(typtr++) = y_temp / krmax;

                //abs(g(:)
                double abs_w = sqrt((pow(xgrad[gradcount], 2)) + (pow(ygrad[gradcount], 2)));
                double ang_g = xgrad[gradcount] == 0.0 ? PI/2 : atan2(ygrad[gradcount],xgrad[gradcount]);

                double ang_t = x_tr == 0.0 ? PI/2 : atan2(y_tr,x_tr);

                double tp_w = sin(ang_g - ang_t);
                tp_w = abs_w * abs(tp_w);
                *wptr++ = tp_w;
            }
        }
    }
    hoNDArray<float> calculate_weights(const hoNDArray<floatd2> &gradients, const hoNDArray<floatd2> &trajectories) {

        hoNDArray<float> weights(gradients.dimensions());

        boost::transform(gradients,trajectories,weights.begin(),
                [](const floatd2& gradient,const floatd2& trajectory){
                    auto abs_w = norm(gradient);
                    auto ang_g = atan2(gradient[1],gradient[0]);
                    auto ang_t = atan2(trajectory[1],trajectory[0]);
                    return abs(sin(ang_g-ang_t))*abs_w*2;

        });

        return weights;
    }

    hoNDArray<float> calculate_weights_Hoge(const hoNDArray<floatd2> &gradients, const hoNDArray<floatd2> &trajectories) {

        hoNDArray<float> weights(gradients.dimensions());

        boost::transform(gradients,trajectories,weights.begin(),
                [](const floatd2& gradient,const floatd2& trajectory){
                    auto abs_g = norm(gradient);
                    auto abs_t = norm(trajectory);
                    auto ang_g = atan2(gradient[1],gradient[0]);
                    auto ang_t = atan2(trajectory[1],trajectory[0]);
                    return abs(cos(ang_g-ang_t))*abs_g*abs_t;

        });

        return weights;
    }

    hoNDArray<floatd2> calculate_trajectories(const hoNDArray<floatd2> &gradients, float sample_time, float krmax) {
                const int nints = gradients.get_size(1);
        const int ngrad = gradients.get_size(0);

        auto trajectory = hoNDArray < floatd2 > (gradients.dimensions());

        for (int interleave = 0; interleave < nints; interleave++) {
            trajectory(0, interleave) = floatd2(0, 0);

            for (int gradcount = 1; gradcount < ngrad; gradcount++) {
                trajectory(gradcount, interleave) = trajectory(gradcount - 1, interleave) +
                                                    float(GAMMA) * gradients(gradcount - 1,interleave) * sample_time;
            }
        }

        boost::transform(trajectory,trajectory.begin(),[&](auto t){ return t/krmax;});
        return trajectory;
    }

    hoNDArray<floatd2> calculate_vds(double slewmax, double gradmax, double Tgsample, double Tdsample, int Ninterleaves,
             double *fov, int numfov, double krmax, int ngmax,int max_nsamples){

        double* x_ptr;
        double* y_ptr;
        int Nints;
        calc_vds(slewmax,gradmax,Tgsample,Tdsample,Ninterleaves,fov, numfov,krmax,ngmax,&x_ptr,&y_ptr,&Nints);

        hoNDArray<floatd2> gradient(std::min(Nints,max_nsamples));
        size_t nsamples = gradient.get_number_of_elements();
        std::transform(x_ptr,x_ptr+nsamples,y_ptr,gradient.begin(),[](auto x, auto y){return -floatd2(x,y)/2;});

        delete[] x_ptr;
        delete[] y_ptr;
        return gradient;

    }

    hoNDArray<floatd2>  create_rotations(const hoNDArray<floatd2>& trajectories,int nints) {
        auto dims = trajectories.dimensions();
        dims.push_back(nints);
         hoNDArray < floatd2 > result(dims);

        int ngrad = trajectories.get_number_of_elements();
        for (int inter = 0; inter < nints; inter++) {
            double rotation = (inter * 2 * PI) / nints;
            for (int gradcount = 0; gradcount < ngrad; gradcount++) {
                floatd2 point = trajectories[gradcount];
                result(gradcount, inter) = floatd2(point[0] * cos(rotation) + point[1] * sin(rotation),
                                                  -point[0] * sin(rotation) + point[1] * cos(rotation));
            }
        }
        return result;
    }
}


#include <array>
#include <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

using state_type = array<double, 3>;

void lorenz_param(const state_type& x, state_type& dxdt, double t,
                  double sigma, double R, double b) {
    dxdt[0] = sigma*(x[1] - x[0]);
    dxdt[1] = R*x[0] - x[1] - x[0]*x[2];
    dxdt[2] = -b*x[2] + x[0]*x[1];
}

void write_lorenz(const state_type& x, const double t) {
    cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2]
         << endl;
}

int main(int argc, char **argv)
{
    const double sigma = 10.0;
    const double R = 28.0;
    const double b = 8.0/3.0;
    using namespace std::placeholders;
    auto lorenz = bind(lorenz_param, _1, _2, _3, sigma, R, b);
    state_type x = { 10.0, 1.0, 1.0 }; // initial conditions
    integrate(lorenz, x, 0.0, 25.0, 0.1, write_lorenz );
    return 0;
}


#include <iomanip>
#include <iostream>
#include <armadillo>

#include "GordonNewell.h"

using namespace std;
using namespace arma;

void writeOutput(ofstream& ofstream, const mat& matrix, int K)
{
    ofstream << "////////////////////////////////////////" << endl;
    ofstream << "////////////////" << " K := " << K << " ////////////////" << endl;
    ofstream << "////////////////////////////////////////" << endl;

    ofstream << endl;

    for (int i = 0; i < matrix.n_rows; i++)
        ofstream << "X_" << i + 1 << " = " << matrix(i, 0) << endl;

    ofstream << endl;
}

void writeOutputBuzen(ofstream& ofstream, const mat& data, const int k, const int n, const double& T)
{
    ofstream << "////////////////////////////////////////" << endl;
    ofstream << "///////////" << " K := " << k << "; N := " << n << "; ///////////" << endl;
    ofstream << "////////////////////////////////////////" << endl;

    ofstream << endl;

    ofstream << "T = " << T << " [s]" << endl;

    ofstream << endl;
    ofstream << left
        << setw(15) << "Server #"
        << setw(15) << "U"
        << setw(15) << "X [1/s]"
        << setw(15) << "n_avg"
        << setw(15) << "R [s]"
        << endl;

    for (int i = 0; i < data.n_rows; i++)
    {
        ofstream << left << setw(15) << i;

        for (int j = 0; j < data.n_cols; j++)
            ofstream << left << setw(15) << data(i, j);

        ofstream << endl;
    }

    ofstream << endl;
}

int main(int argc, char** argv)
{
    ofstream outputStream;
    outputStream.open("potraznje_analiticki.txt");

    ofstream output10, output15, output20;
    output10.open("rezultati_analiticki_10.txt");
    output15.open("rezultati_analiticki_15.txt");
    output20.open("rezultati_analiticki_20.txt");

    for (int k = 2; k <= 8; k++)
    {
        Analyzer gn(k);
        mat result = gn.GetXMatrix();

        writeOutput(outputStream, result, k);

        for (int mprg = 10; mprg <= 20; mprg += 5)
        {
            double T;
            mat data = gn.GetBuzenOutput(result, mprg, T);

            ofstream* oref;
            if (mprg == 10)
                oref = &output10;
            else if (mprg == 15)
                oref = &output15;
            else
                oref = &output20;

            writeOutputBuzen(*oref, data, k, mprg, T);
        }
    }

    outputStream.close();
    output10.close();
    output15.close();
    output20.close();

    return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/quantlib.hpp>

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>

using namespace std;
using namespace QuantLib;

#ifdef BOOST_MSVC
#  ifdef QL_ENABLE_THREAD_SAFE_OBSERVER_PATTERN
#    include <ql/auto_link.hpp>
#    define BOOST_LIB_NAME boost_system
#    include <boost/config/auto_link.hpp>
#    undef BOOST_LIB_NAME
#    define BOOST_LIB_NAME boost_thread
#    include <boost/config/auto_link.hpp>
#    undef BOOST_LIB_NAME
#  endif
#endif

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        /*********************
         ***  MARKET DATA  ***
         *********************/

        Calendar calendar = TARGET();
        Date todaysDate(15, May, 2007);
        // must be a business day
        todaysDate = calendar.adjust(todaysDate);

        Settings::instance().evaluationDate() = todaysDate;

        // dummy curve
        boost::shared_ptr<Quote> flatRate(new SimpleQuote(0.01));
        Handle<YieldTermStructure> tsCurve(
              boost::shared_ptr<FlatForward>(
                      new FlatForward(todaysDate, Handle<Quote>(flatRate),
                                      Actual365Fixed())));

        /*
          In Lehmans Brothers "guide to exotic credit derivatives"
          p. 32 there's a simple case, zero flat curve with a flat CDS
          curve with constant market spreads of 150 bp and RR = 50%
          corresponds to a flat 3% hazard rate. The implied 1-year
          survival probability is 97.04% and the 2-years is 94.18%
        */

        // market
        Real recovery_rate = 0.5;
        Real quoted_spreads[] = { 0.0150, 0.0150, 0.0150, 0.0150 };
        vector<Period> tenors;
        tenors.push_back(3*Months);
        tenors.push_back(6*Months);
        tenors.push_back(1*Years);
        tenors.push_back(2*Years);
        vector<Date> maturities;
        for (Size i=0; i<4; i++) {
            maturities.push_back(calendar.adjust(todaysDate + tenors[i],
                                                 Following));
        }

        std::vector<boost::shared_ptr<DefaultProbabilityHelper> > instruments;
        for (Size i=0; i<4; i++) {
            instruments.push_back(boost::shared_ptr<DefaultProbabilityHelper>(
                new SpreadCdsHelper(
                              Handle<Quote>(boost::shared_ptr<Quote>(
                                         new SimpleQuote(quoted_spreads[i]))),
                              tenors[i],
                              0,
                              calendar,
                              Quarterly,
                              Following,
                              DateGeneration::TwentiethIMM,
                              Actual365Fixed(),
                              recovery_rate,
                              tsCurve)));
        }

        // Bootstrap hazard rates
        boost::shared_ptr<PiecewiseDefaultCurve<HazardRate, BackwardFlat> >
           hazardRateStructure(
               new PiecewiseDefaultCurve<HazardRate, BackwardFlat>(
                                                           todaysDate,
                                                           instruments,
                                                           Actual365Fixed()));
        vector<pair<Date, Real> > hr_curve_data = hazardRateStructure->nodes();

        cout << "Calibrated hazard rate values: " << endl ;
        for (Size i=0; i<hr_curve_data.size(); i++) {
            cout << "hazard rate on " << hr_curve_data[i].first << " is "
                 << hr_curve_data[i].second << endl;
        }
        cout << endl;

        cout << "Some survival probability values: " << endl ;
        cout << "1Y survival probability: "
             << io::percent(hazardRateStructure->survivalProbability(
                                                        todaysDate + 1*Years))
             << endl
             << "               expected: "
             << io::percent(0.9704)
             << endl;
        cout << "2Y survival probability: "
             << io::percent(hazardRateStructure->survivalProbability(
                                                        todaysDate + 2*Years))
             << endl
             << "               expected: "
             << io::percent(0.9418)
             << endl;

        cout << endl << endl;

        // reprice instruments
        Real nominal = 1000000.0;
        Handle<DefaultProbabilityTermStructure> probability(hazardRateStructure);
        boost::shared_ptr<PricingEngine> engine(
                  new MidPointCdsEngine(probability, recovery_rate, tsCurve));

        Schedule cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[0])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_3m(Protection::Seller,
                                 nominal,
                                 quoted_spreads[0],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[1])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_6m(Protection::Seller,
                                 nominal,
                                 quoted_spreads[1],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[2])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_1y(Protection::Seller,
                                 nominal,
                                 quoted_spreads[2],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[3])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_2y(Protection::Seller,
                                 nominal,
                                 quoted_spreads[3],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cds_3m.setPricingEngine(engine);
        cds_6m.setPricingEngine(engine);
        cds_1y.setPricingEngine(engine);
        cds_2y.setPricingEngine(engine);

        cout << "Repricing of quoted CDSs employed for calibration: " << endl;
        cout << "3M fair spread: " << io::rate(cds_3m.fairSpread()) << endl
             << "   NPV:         " << cds_3m.NPV() << endl
             << "   default leg: " << cds_3m.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_3m.couponLegNPV() << endl
             << endl;

        cout << "6M fair spread: " << io::rate(cds_6m.fairSpread()) << endl
             << "   NPV:         " << cds_6m.NPV() << endl
             << "   default leg: " << cds_6m.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_6m.couponLegNPV() << endl
             << endl;

        cout << "1Y fair spread: " << io::rate(cds_1y.fairSpread()) << endl
             << "   NPV:         " << cds_1y.NPV() << endl
             << "   default leg: " << cds_1y.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_1y.couponLegNPV() << endl
             << endl;

        cout << "2Y fair spread: " << io::rate(cds_2y.fairSpread()) << endl
             << "   NPV:         " << cds_2y.NPV() << endl
             << "   default leg: " << cds_2y.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_2y.couponLegNPV() << endl
             << endl;

        cout << endl << endl;

        Real seconds  = timer.elapsed();
        Integer hours = Integer(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = Integer(seconds/60);
        seconds -= minutes * 60;
        cout << "Run completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s" << endl;

        return 0;
    } catch (exception& e) {
        cerr << e.what() << endl;
        return 1;
    } catch (...) {
        cerr << "unknown error" << endl;
        return 1;
    }
}