#include <graphene/singularity/scan.hpp>
#include <queue>
#include <boost/graph/graphviz.hpp>

using namespace boost;
using namespace boost::numeric::ublas;
using namespace singularity;

scan::scan(double parameter_e, uint parameter_m) {
    this->parameter_e = parameter_e;
    this->parameter_m = parameter_m;
}

void scan::process(Graph& g) {
    calculate_similarity(g);
    calculate_neighbours(g);
    find_clusters(g);
}

void scan::calculate_neighbours(Graph& g)
{
    calculate_neighbours_partial(g, range_t(0, g.m_vertices.size()));
}

void scan::calculate_neighbours_partial(Graph& g, range_t r) {
    Graph::vertex_iterator current, end;
    tie(current, end) = vertices(g);
    for (;current != end; current++) {
        unsigned int id = get(vertex_index, g, *current);
        if (id >= r.start() && id < r.start() + r.size()) {
            calculate_neighbours(g, *current);
        }
    }
}

void scan::calculate_neighbours(Graph& g, Graph::vertex_descriptor vertex)
{
    Graph::out_edge_iterator current, end;
    tie(current, end) =  out_edges(vertex, g);
    unsigned int neighbours_count = 0;
    for(; current != end; current++) {
        bool structural_similarity_is_high = get(edge_similarity_is_high, g, *current);
        if (structural_similarity_is_high) {
            neighbours_count++;
        }
    }

    bool is_core;

    if (neighbours_count >= parameter_m) {
        is_core = true;
    } else {
        is_core = false;
    }
    put(vertex_is_core, g, vertex, is_core);
    put(vertex_neighbour_count, g, vertex, neighbours_count);
}

void scan::calculate_similarity(Graph& g)
{
    calculate_similarity_partial(g, range_t(0,g.m_edges.size()));
}

void scan::calculate_similarity_partial(Graph& g, range_t r)
{
    Graph::edge_iterator current, end;
    tie(current, end) = edges(g);
    for (;current != end; current++) {
        unsigned int id = get(edge_index, g, *current);
        if (id >= r.start() && id < r.start() + r.size()) {
            calculate_similarity(g, *current);
        }
    }
}

void scan::calculate_similarity(Graph &g, Graph::edge_descriptor link)
{
    double parameter_e_sq = parameter_e * parameter_e;

    Graph::vertex_descriptor v1 = source(link, g), v2 = target(link, g);

    Graph::degree_size_type v_1_N=0, v_2_N=0, v_12_N=0;

    v_1_N = out_degree(v1, g) + 1;
    v_2_N = out_degree(v2, g) + 1;
    v_12_N = 2;

    Graph::adjacency_iterator current_it, end_it;

    tie( current_it, end_it ) = adjacent_vertices(v1, g);

    for (; current_it < end_it; current_it++) {
        bool found;
        Graph::edge_descriptor link2x;
        Graph::vertex_descriptor vx = *current_it;
        tie(link2x, found) = edge(v2, vx, g);
        if (found) {
            v_12_N++;
        }
    }

    double similarity = (double) (v_12_N * v_12_N) / (v_1_N * v_2_N);

    put(edge_similarity, g, link, similarity);
    put(edge_similarity_is_high, g, link, similarity >= parameter_e_sq);
}

void scan::find_clusters(Graph& g)
{
    unsigned int new_cluster_id = 0;

    std::queue<Graph::vertex_descriptor> q;

    auto vertex_cluster_id_map = get(vertex_cluster_id, g);
    auto vertex_status_map = get(vertex_status, g);
    auto vertex_is_core_map = get(vertex_is_core, g);
    auto edge_similarity_is_high_map = get(edge_similarity_is_high, g);

    Graph::vertex_iterator current, end;

    tie(current, end) = vertices(g);

    for ( ; current != end; current++ ) {
        vertex_status_map[*current] = node_status_unclassified;
    }

    tie(current, end) = vertices(g);

    id_generator gen;

    for ( ; current != end; current++ ) {
        Graph::vertex_descriptor v =  *current;

        if (vertex_status_map[v] != node_status_unclassified) {
            continue;
        }

        if (vertex_is_core_map[v]) {
            new_cluster_id = gen.get_next_id();
            vertex_cluster_id_map[v] = new_cluster_id;
            vertex_status_map[v] = node_status_member;
            q.push(v);

            while (q.size() > 0) {
                Graph::vertex_descriptor y = q.front();

                if (vertex_is_core_map[y]) {
                    Graph::out_edge_iterator current_edge, end_edge;
                    tie(current_edge, end_edge) = out_edges(y, g);
                    for (; current_edge != end_edge; current_edge++ ) {
                        bool similarity_is_high = edge_similarity_is_high_map[*current_edge];
                        if (similarity_is_high) {
                            Graph::vertex_descriptor x = target(*current_edge, g);
                            node_status_t status = vertex_status_map[x];
                            if (status == node_status_unclassified || status == node_status_non_member) {
                                vertex_cluster_id_map[x] = new_cluster_id;
                                vertex_status_map[x] = node_status_member;
                            }
                            if (status == node_status_unclassified) {
                                q.push(x);
                            }
                        }
                    }
                }

                q.pop();
            }
        } else {
            vertex_status_map[v] = node_status_non_member;
        }
    }

    tie(current, end) = vertices(g);

    for ( ; current != end; current++ ) {
        Graph::vertex_descriptor v =  *current;
        node_status_t status = vertex_status_map[v];
        if (status == node_status_non_member) {
            new_cluster_id = gen.get_next_id();
            vertex_cluster_id_map[v] = new_cluster_id;
            bool cluster_is_found = false;
            unsigned int found_cluster_id;
            bool node_is_hub = false;
            Graph::out_edge_iterator current_edge, end_edge;
            tie(current_edge, end_edge) = out_edges(v, g);
            for (; current_edge != end_edge; current_edge++ ) {
                Graph::vertex_descriptor x = target(*current_edge, g);
                if (vertex_status_map[x] == node_status_member) {
                    if (!cluster_is_found) {
                        cluster_is_found = true;
                        found_cluster_id = vertex_cluster_id_map[x];
                    } else {
                        if (found_cluster_id != vertex_cluster_id_map[x]) {
                            node_is_hub = true;
                            break;
                        }
                    }
                }
            }

            vertex_status_map[v] = node_is_hub ? node_status_hub : node_status_outlier;
        }
    }

    set_property(g, graph_num_clusters, new_cluster_id + 1);
}

id_generator::id_generator() {
    init();
}

void id_generator::init() {
    current_id = 0;
}

unsigned int id_generator::get_next_id() {
    return current_id ++;
}

void scan::print_graph(Graph& g) {

//
//    dynamic_properties dp;
//
//
//    Graph::vertex_iterator current, end;
//
//    tie (current, end) = vertices(g);
//    for (; current != end; current++) {
//        unsigned int index = get(vertex_index, g, *current);
//        unsigned int cluster_id = get(vertex_cluster_id, g, *current);
//        node_status_t status = get(vertex_status, g, *current);
//        std::string name;
//        switch (status) {
//            case node_status_member:
//                name = std::string("member");
//                break;
//            case node_status_hub:
//                name = std::string("hub");
//                break;
//            case node_status_outlier:
//                name = std::string("outlier");
//                break;
//            default:
//                name = std::string("?");
//        }
//
//        std::string format = std::string("%d (%d) %s");
//        put(vertex_name, g, *current, name);
//    }
//
//    dp.property("similarity", boost::get(edge_similarity, g));
//    dp.property("neighbours", boost::get(vertex_neighbour_count, g));
//    dp.property("node_id", boost::get(vertex_index, g));
//    dp.property("is_core", boost::get(vertex_is_core, g));
//    dp.property("cluster_id", boost::get(vertex_cluster_id, g));
//    dp.property("status", boost::get(vertex_name, g));
//
//    write_graphviz_dp(std::cout, g, dp);

    property_map<Graph, vertex_cluster_id_t>::type vertex_cluster_id_map = get(vertex_cluster_id, g);
    property_map<Graph, vertex_index_t>::type vertex_id_map = get(vertex_index, g);
    property_map<Graph, edge_similarity_t>::type edge_similarity_map = get(edge_similarity, g);

    dynamic_properties dp;

    dp.property("cluster_id", vertex_cluster_id_map);
    dp.property("id", vertex_id_map);
    dp.property("similarity", edge_similarity_map);

    write_graphviz_dp(std::cout, g, dp, "id");
}


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 2019/12/24 20:41:23
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes(int i)
{
  cout << i << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

int H, W;
vector<string> S;

void stamp(int x, int y)
{

#if DEBUG == 1
  cerr << "stamp(" << x << ", " << y << ")" << endl;
#endif
  for (auto i = x; i < min(H, x + H / 2); i++)
  {
    for (auto j = y; j < min(W, y + W / 2); j++)
    {
      S[i][j] = '#';
#if DEBUG == 1
      cerr << "S[" << i << "][" << j << "]" << endl;
#endif
    }
  }
}

int main()
{
  cin >> H >> W;
  S.resize(H);
  for (auto i = 0; i < H; i++)
  {
    cin >> S[i];
  }
  set<int> X, Y;
  for (auto i = 0; i < H; i++)
  {
    for (auto j = 0; j < W; j++)
    {
      if (S[i][j] == '.')
      {
        X.insert(i);
        Y.insert(j);
      }
    }
  }
  if (X.empty())
  {
    Yes(0);
  }
  auto it = X.begin();
  int ubh{*it};
  it = X.end();
  it--;
  int lbh{*it};
  it = Y.begin();
  int ubw{*it};
  it = Y.end();
  it--;
  int lbw{*it};
  int height{lbh - ubh + 1};
  int width{lbw - ubw + 1};
  if (height <= H / 2 && width <= W / 2)
  {
    Yes(1);
  }
  if (height <= H / 2)
  {
    Yes(2);
  }
  if (width <= W / 2)
  {
    Yes(2);
  }
  int cnt{0};
  for (auto k = 0; k < H + W - 1; k++)
  {
    for (auto i = 0; i <= k; i++)
    {
      auto j = k - i;
      if (!(i < H && j < W))
      {
        continue;
      }
      if (0 <= i && i < H && 0 <= j && j < W && S[i][j] == '.')
      {
        ++cnt;
        stamp(i, j);
      }
      auto ni = H - 1 - i;
      auto nj = W - 1 - j;
      if (0 <= ni && ni < H && 0 <= j && j < W && S[ni][j] == '.')
      {
        ++cnt;
        stamp(ni - H / 2 + 1, j);
      }
      if (0 <= i && i < H && 0 <= nj && nj < W && S[i][nj] == '.')
      {
        ++cnt;
        stamp(i, nj - W / 2 + 1);
      }
      if (0 <= ni && ni < H && 0 <= nj && nj < W && S[ni][nj] == '.')
      {
        ++cnt;
        stamp(ni - H / 2 + 1, nj - W / 2 + 1);
      }
    }
  }
  if (cnt > 4)
  {
    assert(false);
  }
  cout << cnt << endl;
}


//=====================================================
// File   :  ublas_interface.hh
// Author :  L. Plagne <laurent.plagne@edf.fr)>
// Copyright (C) EDF R&D,  lun sep 30 14:23:27 CEST 2002
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// 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
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef UBLAS_INTERFACE_HH
#define UBLAS_INTERFACE_HH

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>

using namespace boost::numeric;

template <class real>
class ublas_interface{

public :

  typedef real real_type ;

  typedef std::vector<real> stl_vector;
  typedef std::vector<stl_vector> stl_matrix;

  typedef typename boost::numeric::ublas::matrix<real,boost::numeric::ublas::column_major> gene_matrix;
  typedef typename boost::numeric::ublas::vector<real> gene_vector;

  static inline std::string name( void ) { return "ublas"; }

  static void free_matrix(gene_matrix & A, int N) {}

  static void free_vector(gene_vector & B) {}

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    A.resize(A_stl.size(),A_stl[0].size());
    for (int j=0; j<A_stl.size() ; j++)
      for (int i=0; i<A_stl[j].size() ; i++)
        A(i,j)=A_stl[j][i];
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    B.resize(B_stl.size());
    for (int i=0; i<B_stl.size() ; i++)
      B(i)=B_stl[i];
  }

  static inline void vector_to_stl(gene_vector & B, stl_vector & B_stl){
    for (int i=0; i<B_stl.size() ; i++)
      B_stl[i]=B(i);
  }

  static inline void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){
    int N=A_stl.size();
    for (int j=0;j<N;j++)
    {
      A_stl[j].resize(N);
      for (int i=0;i<N;i++)
        A_stl[j][i]=A(i,j);
    }
  }

  static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){
    for (int i=0;i<N;i++){
      cible(i) = source(i);
    }
  }

  static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){
    for (int i=0;i<N;i++){
      for (int j=0;j<N;j++){
        cible(i,j) = source(i,j);
      }
    }
  }

  static inline void matrix_vector_product_slow(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X =  prod(A,B);
  }

  static inline void matrix_matrix_product_slow(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N){
    X =  prod(A,B);
  }

  static inline void axpy_slow(const real coef, const gene_vector & X, gene_vector & Y, int N){
    Y+=coef*X;
  }

  // alias free assignments

  static inline void matrix_vector_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X.assign(prod(A,B));
  }

  static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X.assign(prod(trans(A),B));
  }

  static inline void matrix_matrix_product(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N){
    X.assign(prod(A,B));
  }

  static inline void axpy(const real coef, const gene_vector & X, gene_vector & Y, int N){
    Y.plus_assign(coef*X);
  }

  static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int N){
    Y = a*X + b*Y;
  }

  static inline void ata_product(gene_matrix & A, gene_matrix & X, int N){
    // X =  prod(trans(A),A);
    X.assign(prod(trans(A),A));
  }

  static inline void aat_product(gene_matrix & A, gene_matrix & X, int N){
    // X =  prod(A,trans(A));
    X.assign(prod(A,trans(A)));
  }

  static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector & X, int N){
    X = solve(L, B, ublas::lower_tag ());
  }

};

#endif


//ibd_m.cpp equivalent cpp script of ibd_m.py
#include <boost/python.hpp>
#include<iostream>
#include<vector>
#include<algorithm>
#include<map>
#include<string>
using namespace std;
typedef std::vector<float> VecFloat;
typedef std::vector<string> VecString;
typedef std::vector<int> VecInt;
typedef std::vector<int> VecInt;
typedef std::vector<VecInt> VecVecInt;
VecFloat sib_ibd(VecString &geno);
float prob_ibd(VecString &geno, string &allele);
float cousin_ibd(VecString &geno);
float un_ibd(VecString &geno);

VecFloat sib_ibd(VecString &geno)
{
	VecFloat ibd (3,0.0);
	VecVecInt inherit;
	std::vector<VecVecInt> compat_inherit;
	for (VecString::iterator it=geno.begin()+4; it<geno.end(); it++)
	{
		//for each allele in offspring
		string sib_allele = *it;
		VecInt tmp_inheirt;
		VecString::iterator pos=geno.begin()-1;
		while (pos != geno.begin()+4)
		{
			pos = std::find(pos+1,geno.begin()+4,sib_allele);
			if ((pos-geno.begin()) !=4){tmp_inheirt.push_back(pos-geno.begin());}
		}
		inherit.push_back(tmp_inheirt);
	}
	for (int indiv=0; indiv<2; indiv++)
	{
		//for each offspring
		VecVecInt sib_inherit;
		for (VecInt::iterator tmp_it=inherit[2*indiv].begin();tmp_it<inherit[2*indiv].end();tmp_it++)
		{
			for (VecInt::iterator tmp2_it=inherit[2*indiv+1].begin();tmp2_it<inherit[2*indiv+1].end();tmp2_it++)
			{
				if (*tmp_it<2 && *tmp2_it>1 || *tmp_it>1 && *tmp2_it<2)
				{
					VecInt temp {*tmp_it,*tmp2_it};
					sib_inherit.push_back(temp);
				}
			}
		}
		compat_inherit.push_back(sib_inherit);
	}
	float count[3] = {0.0,0.0,0.0};
	int count_sum = compat_inherit[0].size()*compat_inherit[1].size();
	for (VecVecInt::iterator tmp1_it=compat_inherit[0].begin(); tmp1_it<compat_inherit[0].end(); tmp1_it++)
	{
		//count IBD value
		for (VecVecInt::iterator tmp2_it=compat_inherit[1].begin(); tmp2_it<compat_inherit[1].end(); tmp2_it++)
		{
			int ibd_count=0;
			VecInt tmp_common;
			for (VecInt::iterator i = tmp1_it->begin(); i != tmp1_it->begin()+2;i++)
			{
				if (std::find(tmp2_it->begin(),tmp2_it->begin()+2,*i) != tmp2_it->begin()+2 && geno.at(*i) != "1")
				{
					tmp_common.push_back(*i);//shared allele in [0,1,2,3]
				}
			}
			ibd_count = tmp_common.size();
			count[ibd_count]++;
		}
	}
	for (int i=0; i<3; i++){ibd[i]=count[i]/count_sum;}
	return ibd;
}

float prob_ibd(VecString &geno, string &allele)
{
	VecInt pos;   //position of the given allele in parental genotypes
	VecVecInt alt_pos;
	float prob=0.0;
	VecString alt {"-1","-1"};
	for (auto iter=geno.begin(); iter != geno.begin()+4; iter++)
	{
		if (*iter == allele){pos.push_back((iter-geno.begin()));}
	}
	for (auto iter=geno.begin()+4; iter != geno.end(); iter++)
	{
		//determine the alternative allele
		int tmp_pos = iter-(geno.begin()+4);
		if (*iter != allele)
		{
			if (tmp_pos<2){alt[0]=*iter;}
			else {alt[1]=*iter;}
		}
		else if (tmp_pos%2==1 && alt[tmp_pos/2]=="-1"){alt[tmp_pos/2]=*iter;}
	}
	VecInt altpos1, altpos2;
	for (auto iter=geno.begin(); iter != geno.begin()+4; iter++)
	{
		if (*iter == alt[0]){altpos1.push_back((iter-geno.begin()));}
		if (*iter == alt[1]){altpos2.push_back((iter-geno.begin()));}
	}
	alt_pos.push_back(altpos1);
	alt_pos.push_back(altpos2);
	vector<map<int,float>> freq_map;
	for (int i=0;i<2;i++)
	{
		map<int,float> tmp_occurrence;
		VecInt tmp_p_compat;
		for (auto p : pos)
		{
			for (auto q : alt_pos[i])
			{
				if (p<2 && q>1 || p>1 && q<2){tmp_p_compat.push_back(p);}
			}
		}
		for (auto tmp : tmp_p_compat)
		{
			float count = (float) 1/tmp_p_compat.size();
			if(tmp_occurrence.find(tmp)==tmp_occurrence.end())
			{
				//not in key
				tmp_occurrence[tmp]=count;
			}
			else {tmp_occurrence[tmp]+=count;}
		}
		freq_map.push_back(tmp_occurrence);
	}
	for (auto &it1 : freq_map[0])
	{
		for (auto &it2 : freq_map[1])
		{
			if (it2.first==it1.first){prob+=it1.second*it2.second;}
		}
	}
	return prob;
}

float cousin_ibd(VecString &geno)
{
	//calculate IBD between cousins
	//geno = GT for [grandparents, fam1, fam2] important parent put in the first place of each family
	VecString ag = {geno[8],geno[9]};       //GT for 2 cousins
	VecString bg = {geno[14],geno[15]};
	VecString shared_allele;
	float total_p = 0.0;
	for (auto ait : ag)
	{
		if (std::find(bg.begin(),bg.end(),ait) != bg.end() && ait != "1"){shared_allele.push_back(ait);} //possible shared RV
	}
	if (shared_allele.size()==2 && shared_allele[0]==shared_allele[1]){shared_allele.pop_back();}
	for (auto a : shared_allele)
	{
		int rep=0;
		for (auto tmp : ag)
		{
			for (auto tmp1 : bg)
			{
				if (tmp==a && tmp1==a){rep++;}
			}
		}
		VecFloat f_inherit;
		int flag=0;
		for (int fid=0; fid<2; fid++)
		{
			VecVecInt possible_inherit;
			VecString pg = {geno[fid*6+4],geno[fid*6+5],geno[fid*6+6],geno[fid*6+7]};     //parental genotypes
			string alt="-1";
			VecInt pos, pos_alt;
			if (fid==0)
			{
				for (auto ait=ag.begin(); ait != ag.end(); ait++)
				{
					if (*ait != a)
					{
						alt=*ait;
						break;
					}
					else if (ait-ag.begin()==1)
					{
						alt=*ait;
					}
				}
			}
			if (fid==1)
			{
				for (auto bit=bg.begin(); bit != bg.end(); bit++)
				{
					if (*bit != a)
					{
						alt=*bit;
						break;
					}
					else if (bit-bg.begin()==1)
					{
						alt=*bit;
					}
				}
			}
			for (auto it = pg.begin(); it !=pg.end(); it++)
			{
				if (*it == a) {pos.push_back((it-pg.begin()));}
				if (*it == alt) {pos_alt.push_back((it-pg.begin()));}
			}
			for (auto p : pos)
			{
				for (auto q : pos_alt)
				{
					if (p<2 && q>1 || p>1 && q<2)
					{
						VecInt tmp_pos={p,q};
						//cout<<p<<" "<<q<<endl;
						possible_inherit.push_back(tmp_pos);
					}
				}
			}
			int count=0;
			for (auto inherit : possible_inherit)
			{
				if (inherit[0]<2){count++;}
			}
			float f_in;
			if (possible_inherit.size()>0)
			{
				f_in = (float)count/possible_inherit.size();
			}
			else {f_in=0;}
			f_inherit.push_back(f_in);
			if (f_in>0){flag++;}     //family having a possibility to inherit the particular allele
			if (flag==2)
			{
				//get the GT for upper nuclear family
				VecString up_gt={geno[0],geno[1],geno[2],geno[3],geno[4],geno[5],geno[10],geno[11]};
				float prob = prob_ibd(up_gt,a);
				total_p += (f_inherit[0]*f_inherit[1]*prob)*rep;
			}
			else {total_p+=0;}
		}
	}
	return total_p;
}

float un_ibd(VecString &geno)
{
	//calculate IBD between Uncle-Nephew pair
	//geno = GT for [grandparents uncle father mother kid(nephew)]
	VecString Uncle_g = {geno[4],geno[5]};
	VecString Nephew_g = {geno[10],geno[11]};
	VecString pg = {geno[6],geno[7],geno[8],geno[9]};
	VecString gt = {geno[0],geno[1],geno[2],geno[3],geno[4],geno[5],geno[6],geno[7]};
	VecString shared_allele;
	float total_p=0.0;
	for (auto g : Uncle_g)
	{
		if (std::find(Nephew_g.begin(),Nephew_g.end(),g) != Nephew_g.end() && g != "1")
		{
			shared_allele.push_back(g);
		}
	}
	if (shared_allele.size()==2 && shared_allele[0]==shared_allele[1]){shared_allele.pop_back();}
	for (auto ref : shared_allele)
	{
		VecString tmp_alleles;
		for (auto b : Nephew_g)
		{
			if (b==ref){tmp_alleles.push_back(b);}
		}
		for (auto allele : tmp_alleles)
		{
			float f=0.0;
			int m=0;
			int n=0;
			string alt="-1";
			for (auto bit=Nephew_g.begin(); bit != Nephew_g.end(); bit++)
			{
				if (*bit != ref)
				{
					alt=*bit;
					break;
				}
				else if (bit-Nephew_g.begin()==1)
				{
					alt=*bit;
				}
			}
			VecInt pos1,pos2;
			for (auto tmp=pg.begin();tmp!=pg.end();tmp++)
			{
				if (*tmp==ref){pos1.push_back((tmp-pg.begin()));}
				if (*tmp==alt){pos2.push_back((tmp-pg.begin()));}
			}
			for (auto p : pos1)
			{
				for (auto q : pos2)
				{
					if (p<2 && q>1){m++;}
					else if (p>1 && q<2){n++;}
				}
			}
			if ((m+n)>0){f=(float)m/(m+n);}
			else{f=0;}
			float prob=0;
			if (f!=0)
			{
				prob=prob_ibd(gt,ref);
			}
			if (Uncle_g[0]==Uncle_g[1] && Uncle_g[0]==ref)
			{
				prob *=2;
			}
			total_p += f*prob;
		}
	}
	return total_p;
}
boost::python::list sib_apply(boost::python::list geno_py)
{
	VecString geno;
	VecFloat ibd;
	boost::python::list ibd_py;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	ibd=sib_ibd(geno);
	for (auto iter : ibd)
	{
		ibd_py.append(iter);
	}
	return ibd_py;
}

float prob_ibd_apply(boost::python::list geno_py,int allele)
{
	VecString geno;
	float prob;
	string str_allele = std::to_string(allele);
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = prob_ibd(geno, str_allele);
	return prob;
}

float cousin_apply(boost::python::list geno_py)
{
	VecString geno;
	float prob;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = cousin_ibd(geno);
	return prob;
}

float un_apply(boost::python::list geno_py)
{
	VecString geno;
	float prob;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = un_ibd(geno);
	return prob;
}
BOOST_PYTHON_MODULE(ibd_rv_cpp){
	using namespace boost::python;
	def("sib_ibd",sib_ibd);
	def("sib_apply",sib_apply);
	def("prob_ibd_apply",prob_ibd_apply);
	def("cousin_apply",cousin_apply);
	def("un_apply",un_apply);
}

/*int main()
{
	VecString geno {"1","1","2","1","1","1","2","1","1","1","1","1"};
	float prob=un_ibd(geno);
	cout<<"IBD:"<<prob<<endl;
	return 0;
}
int main(int argc, char* argv[])
{
	VecString geno;
	VecFloat ibd;
	for (char* i=*(argv+1); *i != '\0';i++)
	{
		geno.push_back(string(1,*i));
	}
	ibd=sib_ibd(geno);
}*/


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  main.cpp
 * @brief Spider trajectory optimization with pre-specified footholds.
 * @Author: Alejandro Escontrela
 * @Author: Stephanie McCormick
 * @Author: Disha Das
 * @Author: Tarushree Gandhi
 */

#include <gtdynamics/dynamics/DynamicsGraph.h>
#include <gtdynamics/dynamics/OptimizerSetting.h>
#include <gtdynamics/factors/MinTorqueFactor.h>
#include <gtdynamics/factors/PointGoalFactor.h>
#include <gtdynamics/universal_robot/Robot.h>
#include <gtdynamics/universal_robot/sdf.h>
#include <gtdynamics/utils/DynamicsSymbol.h>
#include <gtdynamics/utils/initialize_solution_utils.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>

#include <algorithm>
#include <boost/algorithm/string/join.hpp>
#include <boost/optional.hpp>
#include <fstream>
#include <iostream>
#include <utility>

#define GROUND_HEIGHT -1.75

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

using gtsam::Point3;
using gtsam::Pose3;
using gtsam::Rot3;
using gtsam::Values;
using gtsam::Vector;
using gtsam::Vector3;
using gtsam::Vector6;
using gtsam::noiseModel::Isotropic;

using namespace gtdynamics;

int main(int argc, char** argv) {
  // Load Stephanie's spider robot.
  auto robot = gtdynamics::CreateRobotFromFile(kSdfPath + string("spider.sdf"),
                                               "spider");

  double sigma_dynamics = 1e-5;    // std of dynamics constraints.
  double sigma_objectives = 1e-6;  // std of additional objectives.
  double sigma_joints = 1.85e-4;   // 1.85e-4

  // Noise models.
  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
       dynamics_model_3 = Isotropic::Sigma(3, sigma_dynamics),
       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
       dynamics_model_1_2 = Isotropic::Sigma(1, sigma_joints),
       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
       objectives_model_3 = Isotropic::Sigma(3, sigma_objectives),
       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);

  // TODO(aescontrela): Make a constructor for OptimizerSetting that
  //     initializes all noise models with the same sigma.
  auto opt = gtdynamics::OptimizerSetting();
  opt.bp_cost_model = dynamics_model_6;
  opt.bv_cost_model = dynamics_model_6;
  opt.ba_cost_model = dynamics_model_6;
  opt.p_cost_model = dynamics_model_6;
  opt.v_cost_model = dynamics_model_6;
  opt.a_cost_model = dynamics_model_6;
  opt.f_cost_model = dynamics_model_6;
  opt.fa_cost_model = dynamics_model_6;
  opt.t_cost_model = dynamics_model_1;
  opt.cp_cost_model = dynamics_model_1;
  opt.cfriction_cost_model = dynamics_model_1;
  opt.cv_cost_model = dynamics_model_3;
  opt.ca_cost_model = dynamics_model_3;
  opt.planar_cost_model = dynamics_model_3;
  opt.prior_q_cost_model = dynamics_model_1;
  opt.prior_qv_cost_model = dynamics_model_1;
  opt.prior_qa_cost_model = dynamics_model_1;
  opt.prior_t_cost_model = dynamics_model_1;
  opt.q_col_cost_model = dynamics_model_1;
  opt.v_col_cost_model = dynamics_model_1;
  opt.time_cost_model = dynamics_model_1;

  // Env parameters.
  Vector3 gravity(0, 0, -9.8);
  double mu = 1.0;

  auto graph_builder = gtdynamics::DynamicsGraph(opt, gravity);

  vector<string> links = {"tarsus_1_L1", "tarsus_2_L2", "tarsus_3_L3",
                          "tarsus_4_L4", "tarsus_5_R4", "tarsus_6_R3",
                          "tarsus_7_R2", "tarsus_8_R1"};
  // All contacts.
  const Point3 contact_in_com(0, 0.19, 0);
  PointOnLink cp1(robot.link("tarsus_1_L1"), contact_in_com);  // Front left.
  PointOnLink cp2(robot.link("tarsus_2_L2"), contact_in_com);  // Hind left.
  PointOnLink cp3(robot.link("tarsus_3_L3"), contact_in_com);  // Front right.
  PointOnLink cp4(robot.link("tarsus_4_L4"), contact_in_com);  // Hind right.
  PointOnLink cp5(robot.link("tarsus_5_R4"), contact_in_com);  // Front left.
  PointOnLink cp6(robot.link("tarsus_6_R3"), contact_in_com);  // Hind left.
  PointOnLink cp7(robot.link("tarsus_7_R2"), contact_in_com);  // Front right.
  PointOnLink cp8(robot.link("tarsus_8_R1"), contact_in_com);  // Hind right.

  // Contact points for each phase.
  // This gait moves one leg at a time.
  using CPs = PointOnLinks;
  CPs t00 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  // Initially stationary.
  CPs p0 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t01 = {cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs p1 = {cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t12 = {cp3, cp4, cp5, cp6, cp7, cp8};
  CPs p2 = {cp1, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t23 = {cp1, cp4, cp5, cp6, cp7, cp8};
  CPs p3 = {cp1, cp2, cp4, cp5, cp6, cp7, cp8};
  CPs t34 = {cp1, cp2, cp5, cp6, cp7, cp8};
  CPs p4 = {cp1, cp2, cp3, cp5, cp6, cp7, cp8};
  CPs t45 = {cp1, cp2, cp3, cp6, cp7, cp8};
  CPs p5 = {cp1, cp2, cp3, cp4, cp6, cp7, cp8};
  CPs t56 = {cp1, cp2, cp3, cp4, cp7, cp8};
  CPs p6 = {cp1, cp2, cp3, cp4, cp5, cp7, cp8};
  CPs t67 = {cp1, cp2, cp3, cp4, cp5, cp8};
  CPs p7 = {cp1, cp2, cp3, cp4, cp5, cp6, cp8};
  CPs t78 = {cp1, cp2, cp3, cp4, cp5, cp6};
  CPs p8 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7};
  CPs t80 = {cp2, cp3, cp4, cp5, cp6, cp7};

  // This gait moves four legs at a time (alternating tetrapod).
  CPs t0a = {cp2, cp4, cp6, cp8};
  CPs pa = {cp2, cp4, cp6, cp8};
  CPs tab = {};
  CPs pb = {cp1, cp3, cp5, cp7};
  CPs tb0 = {cp1, cp3, cp5, cp7};

  // Define contact points for each phase, transition contact points,
  // and phase durations.
  // Alternating Tetrapod:
  vector<CPs> phase_cps = {p0, pa, p0, pb, p0, pa, p0, pb, p0, pa,
                           p0, pb, p0, pa, p0, pb, p0, pa, p0, pb};
  vector<CPs> trans_cps = {t0a, t0a, tb0, tb0, t0a, t0a, tb0, tb0, t0a, t0a,
                           tb0, tb0, t0a, t0a, tb0, tb0, t0a, t0a, tb0};
  vector<int> phase_steps = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20,
                             20, 20, 20, 20, 20, 20, 20, 20, 20, 20};

  // Define noise to be added to initial values, desired timestep duration,
  // vector of link name strings, robot model for each phase, and
  // phase transition initial values.
  double gaussian_noise = 1e-5;

  double dt_des = 1. / 240;
  vector<Values> transition_graph_init;

  // Define the cumulative phase steps.
  vector<int> cum_phase_steps;
  for (int i = 0; i < phase_steps.size(); i++) {
    int cum_val =
        i == 0 ? phase_steps[0] : phase_steps[i] + cum_phase_steps[i - 1];
    cum_phase_steps.push_back(cum_val);
    std::cout << cum_val << std::endl;
  }
  int t_f = cum_phase_steps[cum_phase_steps.size() - 1];  // Final timestep.

  // Collocation scheme.
  auto collocation = gtdynamics::CollocationScheme::Euler;

  // Graphs for transition between phases + their initial values.
  vector<gtsam::NonlinearFactorGraph> transition_graphs;
  for (int p = 1; p < phase_cps.size(); p++) {
    std::cout << "Creating transition graph" << std::endl;
    transition_graphs.push_back(graph_builder.dynamicsFactorGraph(
        robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu));
    std::cout << "Creating initial values" << std::endl;
    transition_graph_init.push_back(ZeroValues(
        robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1]));
  }

  // Construct the multi-phase trajectory factor graph.
  std::cout << "Creating dynamics graph" << std::endl;
  auto graph = graph_builder.multiPhaseTrajectoryFG(
      robot, phase_steps, transition_graphs, collocation, phase_cps, mu);

  // Build the objective factors.
  gtsam::NonlinearFactorGraph objective_factors;
  auto base_link = robot.link("body");

  std::map<string, gtdynamics::LinkSharedPtr> link_map;
  for (auto&& link : links)
    link_map.insert(std::make_pair(link, robot.link(link)));

  // Previous contact point goal.
  std::map<string, Point3> prev_cp;
  for (auto&& link : links) {
    prev_cp.insert(std::make_pair(link, link_map[link]->bMcom() * cp1.point));
  }

  // Distance to move contact point per time step during swing.
  auto contact_offset = Point3(0, 0.007, 0);

  // Set this to 'right' or 'left' to make the spider rotate in place
  string turn = "right";

  // Add contact point objectives to factor graph.
  for (int p = 0; p < phase_cps.size(); p++) {
    // Phase start and end timesteps.
    int t_p_i = cum_phase_steps[p] - phase_steps[p];
    if (p != 0) t_p_i += 1;
    int t_p_f = cum_phase_steps[p];

    // Obtain the contact links and swing links for this phase.
    vector<string> phase_contact_links;
    for (auto&& cp : phase_cps[p]) {
      phase_contact_links.push_back(cp.link->name());
    }
    vector<string> phase_swing_links;
    for (auto&& l : links) {
      if (std::find(phase_contact_links.begin(), phase_contact_links.end(),
                    l) == phase_contact_links.end()) {
        phase_swing_links.push_back(l);
      }
    }

    for (int t = t_p_i; t <= t_p_f; t++) {
      // Normalized phase progress.
      double t_normed = (double)(t - t_p_i) / (double)(t_p_f - t_p_i);

      for (auto&& pcl : phase_contact_links) {
        // TODO(aescontrela): Use correct contact point for each link.
        // TODO(frank): #179 make sure height is handled correctly.
        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[pcl]->id(), t),
            Isotropic::Sigma(3, 1e-7), cp1.point,
            Point3(prev_cp[pcl].x(), prev_cp[pcl].y(), GROUND_HEIGHT - 0.05)));
      }

      double h =
          GROUND_HEIGHT + std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7);

      for (auto&& psl : phase_swing_links) {
        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[psl]->id(), t),
            Isotropic::Sigma(3, 1e-7), cp1.point,
            Point3(prev_cp[psl].x(), prev_cp[psl].y(), h)));
      }

      // Update the goal point for the swing links.
      for (auto&& psl : phase_swing_links) {
        if (turn.compare("right") == 0) {
          if (psl.find_first_of("1234") != string::npos)
            prev_cp[psl] = prev_cp[psl] + contact_offset;
          else
            prev_cp[psl] = prev_cp[psl] - contact_offset;
        } else {
          if (psl.find_first_of("5678") != string::npos)
            prev_cp[psl] = prev_cp[psl] + contact_offset;
          else
            prev_cp[psl] = prev_cp[psl] - contact_offset;
        }
      }
    }
  }

  // Add base goal objectives to the factor graph.
  for (int t = 0; t <= t_f; t++) {
    objective_factors.add(gtsam::PriorFactor<gtsam::Pose3>(
        internal::PoseKey(base_link->id(), t),
        gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0.0, 0.5)),
        Isotropic::Sigma(6, 6e-5)));  // 6.2e-5
  }

  // Add link boundary conditions to FG.
  for (auto&& link : robot.links()) {
    // Initial link pose, twists.
    objective_factors.add(gtsam::PriorFactor<gtsam::Pose3>(
        internal::PoseKey(link->id(), 0), link->bMcom(), dynamics_model_6));
    objective_factors.add(gtsam::PriorFactor<Vector6>(
        internal::TwistKey(link->id(), 0), Vector6::Zero(), dynamics_model_6));

    // Final link twists, accelerations.
    objective_factors.add(
        gtsam::PriorFactor<Vector6>(internal::TwistKey(link->id(), t_f),
                                    Vector6::Zero(), objectives_model_6));
    objective_factors.add(
        gtsam::PriorFactor<Vector6>(internal::TwistAccelKey(link->id(), t_f),
                                    Vector6::Zero(), objectives_model_6));
  }

  // Add joint boundary conditions to FG.
  for (auto&& joint : robot.joints()) {
    // Add priors to joint angles
    for (int t = 0; t <= t_f; t++) {
      if (joint->name().find("hip_") == 0) {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0, dynamics_model_1_2));
      } else if (joint->name().find("hip2") == 0) {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0.9, dynamics_model_1_2));
      } else if (joint->name().find("knee") == 0) {
        objective_factors.add(
            gtsam::PriorFactor<double>(internal::JointAngleKey(joint->id(), t),
                                       -1.22, dynamics_model_1_2));
      } else {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0.26, dynamics_model_1_2));
      }
    }

    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointVelKey(joint->id(), 0), 0.0, dynamics_model_1));

    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointVelKey(joint->id(), t_f), 0.0, objectives_model_1));
    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointAccelKey(joint->id(), t_f), 0.0, objectives_model_1));
  }

  // Add prior factor constraining all Phase keys to have duration of 1 / 240.
  for (int phase = 0; phase < phase_steps.size(); phase++)
    objective_factors.add(gtsam::PriorFactor<double>(
        PhaseKey(phase), dt_des,
        gtsam::noiseModel::Isotropic::Sigma(1, 1e-30)));

  // Add min torque objectives.
  for (int t = 0; t <= t_f; t++) {
    for (auto&& joint : robot.joints())
      objective_factors.add(gtdynamics::MinTorqueFactor(
          internal::TorqueKey(joint->id(), t),
          gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1)));
  }
  graph.add(objective_factors);

  // Initialize solution.
  gtsam::Values init_vals;
  init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory(
      robot, phase_steps, transition_graph_init, dt_des, gaussian_noise,
      phase_cps);

  // Optimize!
  gtsam::LevenbergMarquardtParams params;
  params.setVerbosityLM("SUMMARY");
  params.setlambdaInitial(1e0);
  params.setlambdaLowerBound(1e-7);
  params.setlambdaUpperBound(1e10);
  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
  auto results = optimizer.optimize();

  vector<string> joint_names;
  for (auto&& joint : robot.joints()) joint_names.push_back(joint->name());
  string joint_names_str = boost::algorithm::join(joint_names, ",");
  std::ofstream traj_file;

  traj_file.open("rotation_traj.csv");
  // angles, vels, accels, torques, time.
  traj_file << joint_names_str << "," << joint_names_str << ","
            << joint_names_str << "," << joint_names_str << ",t"
            << "\n";
  int t = 0;
  for (int phase = 0; phase < phase_steps.size(); phase++) {
    for (int phase_step = 0; phase_step < phase_steps[phase]; phase_step++) {
      vector<string> vals;
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAngle(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointVel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAccel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(Torque(results, joint->id(), t)));
      vals.push_back(std::to_string(results.atDouble(PhaseKey(phase))));
      t++;
      string vals_str = boost::algorithm::join(vals, ",");
      traj_file << vals_str << "\n";
    }
  }
  traj_file.close();

  return 0;

}  // namespace gtdynamics


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/robust_rotation_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <ceres/rotation.h>
#include <unordered_map>

#include "theia/math/l1_solver.h"
#include "theia/math/matrix/sparse_cholesky_llt.h"
#include "theia/math/rotation.h"
#include "theia/sfm/types.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"

namespace theia {

bool RobustRotationEstimator::EstimateRotations(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  for (const auto& view_pair : view_pairs) {
    AddRelativeRotationConstraint(view_pair.first, view_pair.second.rotation_2);
  }
  return EstimateRotations(global_orientations);
}

void RobustRotationEstimator::AddRelativeRotationConstraint(
    const ViewIdPair& view_id_pair, const Eigen::Vector3d& relative_rotation) {
  // Store the relative orientation constraint.
  relative_rotations_.emplace_back(view_id_pair, relative_rotation);
}

bool RobustRotationEstimator::EstimateRotations(
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  CHECK_GT(relative_rotations_.size(), 0)
      << "Relative rotation constraints must be added to the robust rotation "
         "solver before estimating global rotations.";
  global_orientations_ = CHECK_NOTNULL(global_orientations);

  if (fixed_view_ids_.size() == 0) {
    // just set the first rotation fix
    fixed_view_ids_.insert(std::begin(*global_orientations)->first);
    nr_fixed_rotations_ = 1;
  }
  // Compute a mapping of view ids to indices in the linear system. One rotation
  // will have an index of -1 and will not be added to the linear system. This
  // will remove the gauge freedom (effectively holding one camera as the
  // identity rotation).
  int index = 0;
  int fix_index = -nr_fixed_rotations_;
  view_id_to_index_.reserve(global_orientations->size());
  for (const auto& orientation : *global_orientations) {
      if (fixed_view_ids_.find(orientation.first) == fixed_view_ids_.end()) {
          view_id_to_index_[orientation.first] = index;
          ++index;
      } else {
          view_id_to_index_[orientation.first] = fix_index;
          ++fix_index;
      }
  }

  Eigen::SparseMatrix<double> sparse_mat;
  SetupLinearSystem();

  if (!SolveL1Regression()) {
    LOG(ERROR) << "Could not solve the L1 regression step.";
    return false;
  }

  if (!SolveIRLS()) {
    LOG(ERROR) << "Could not solve the least squares error step.";
    return false;
  }

  return true;
}

// Set up the sparse linear system.
void RobustRotationEstimator::SetupLinearSystem() {
  // The rotation change is one less than the number of global rotations because
  // we keep one rotation constant.
  tangent_space_step_.resize((global_orientations_->size() - nr_fixed_rotations_) * 3);
  tangent_space_residual_.resize(relative_rotations_.size() * 3);
  sparse_matrix_.resize(relative_rotations_.size() * 3,
                        (global_orientations_->size() - nr_fixed_rotations_) * 3);

  // For each relative rotation constraint, add an entry to the sparse
  // matrix. We use the first order approximation of angle axis such that:
  // R_ij = R_j - R_i. This makes the sparse matrix just a bunch of identity
  // matrices.
  int rotation_error_index = 0;
  std::vector<Eigen::Triplet<double> > triplet_list;
  for (const auto& relative_rotation : relative_rotations_) {

    // see if this rotation should be fixed
    if (fixed_view_ids_.find(relative_rotation.first.first) == fixed_view_ids_.end()) {
      const int view1_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.first);

      triplet_list.emplace_back(3 * rotation_error_index,
                                3 * view1_index,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view1_index + 1,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view1_index + 2,
                                -1.0);

    }
    if (fixed_view_ids_.find(relative_rotation.first.second) == fixed_view_ids_.end())  {
      const int view2_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.second);
      triplet_list.emplace_back(3 * rotation_error_index + 0,
                                3 * view2_index + 0,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view2_index + 1,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view2_index + 2,
                                1.0);

    }

    ++rotation_error_index;
  }
  sparse_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());
}

bool RobustRotationEstimator::SolveL1Regression() {
  L1Solver<Eigen::SparseMatrix<double> >::Options options;
  options.max_num_iterations = 5;
  L1Solver<Eigen::SparseMatrix<double> > l1_solver(options, sparse_matrix_);

  tangent_space_step_.setZero();
  ComputeResiduals();
  for (int i = 0; i < options_.max_num_l1_iterations; i++) {
    l1_solver.Solve(tangent_space_residual_, &tangent_space_step_);
    UpdateGlobalRotations();
    ComputeResiduals();

    double avg_step_size = ComputeAverageStepSize();

    if (avg_step_size <= options_.l1_step_convergence_threshold) {
      break;
    }
    options.max_num_iterations *= 2;
    l1_solver.SetMaxIterations(options.max_num_iterations);
  }
  return true;
}

bool RobustRotationEstimator::SolveIRLS() {
  const int num_edges = tangent_space_residual_.size() / 3;

  // Set up the linear solver and analyze the sparsity pattern of the
  // system. Since the sparsity pattern will not change with each linear solve
  // this can help speed up the solution time.
  SparseCholeskyLLt linear_solver;
  linear_solver.AnalyzePattern(sparse_matrix_.transpose() * sparse_matrix_);
  if (linear_solver.Info() != Eigen::Success) {
    LOG(ERROR) << "Cholesky decomposition failed.";
    return false;
  }

  VLOG(2) << "Iteration   SqError         Delta";
  const std::string row_format = "  % 4d     % 4.4e     % 4.4e";

  ComputeResiduals();

  Eigen::ArrayXd weights(num_edges * 3);
  Eigen::SparseMatrix<double> at_weight;
  for (int i = 0; i < options_.max_num_irls_iterations; i++) {
    // Compute the Huber-like weights for each error term.
    const double& sigma = options_.irls_loss_parameter_sigma;
    for (int k = 0; k < num_edges; ++k) {
      double e_sq = tangent_space_residual_.segment<3>(3 * k).squaredNorm();
      double tmp = e_sq + sigma * sigma;
      double w = sigma / (tmp * tmp);
      weights.segment<3>(3 * k).setConstant(w);
    }

    // Update the factorization for the weighted values.
    at_weight = sparse_matrix_.transpose() * weights.matrix().asDiagonal();
    linear_solver.Factorize(at_weight * sparse_matrix_);
    if (linear_solver.Info() != Eigen::Success) {
      LOG(ERROR) << "Failed to factorize the least squares system.";
      return false;
    }

    // Solve the least squares problem..
    tangent_space_step_ =
        linear_solver.Solve(at_weight * tangent_space_residual_);
    if (linear_solver.Info() != Eigen::Success) {
      LOG(ERROR) << "Failed to solve the least squares system.";
      return false;
    }

    UpdateGlobalRotations();
    ComputeResiduals();
    const double avg_step_size = ComputeAverageStepSize();

    VLOG(2) << StringPrintf(row_format.c_str(),
                            i,
                            tangent_space_residual_.squaredNorm(),
                            avg_step_size);

    if (avg_step_size < options_.irls_step_convergence_threshold) {
      VLOG(1) << "IRLS Converged in " << i + 1 << " iterations.";
      break;
    }
  }
  return true;
}

// Update the global orientations using the current value in the
// rotation_change.
void RobustRotationEstimator::UpdateGlobalRotations() {
  for (auto& rotation : *global_orientations_) {
    if (fixed_view_ids_.find(rotation.first) != fixed_view_ids_.end()) {
      continue;
    }

    const int view_index = FindOrDie(view_id_to_index_, rotation.first);
    // Apply the rotation change to the global orientation.
    const Eigen::Vector3d& rotation_change =
        tangent_space_step_.segment<3>(3 * view_index);
    rotation.second = MultiplyRotations(rotation.second, rotation_change);
  }
}

// Computes the relative rotation error based on the current global
// orientation estimates.
void RobustRotationEstimator::ComputeResiduals() {
  int rotation_error_index = 0;
  for (const auto& relative_rotation : relative_rotations_) {
    const Eigen::Vector3d& relative_rotation_aa = relative_rotation.second;
    const Eigen::Vector3d& rotation1 =
        FindOrDie(*global_orientations_, relative_rotation.first.first);
    const Eigen::Vector3d& rotation2 =
        FindOrDie(*global_orientations_, relative_rotation.first.second);

    // Compute the relative rotation error as:
    //   R_err = R2^t * R_12 * R1.
    tangent_space_residual_.segment<3>(3 * rotation_error_index) =
        MultiplyRotations(-rotation2,
                          MultiplyRotations(relative_rotation_aa, rotation1));
    ++rotation_error_index;
  }
}

double RobustRotationEstimator::ComputeAverageStepSize() {
  // compute the average step size of the update in tangent_space_step_
  const int numVertices = tangent_space_step_.size() / 3;
  double delta_V = 0;
  for (int k = 0; k < numVertices; ++k) {
    delta_V += tangent_space_step_.segment<3>(3 * k).norm();
  }
  return delta_V / numVertices;
}

bool RobustRotationEstimator::EstimateRotationsWrapper(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    std::unordered_map<ViewId, Eigen::Vector3d>& init_global_orientations) {
    return EstimateRotations(view_pairs, &init_global_orientations);
}

void RobustRotationEstimator::SetFixedGlobalRotations(const std::set<ViewId>& fixed_views) {
    fixed_view_ids_ = fixed_views;
    nr_fixed_rotations_ = fixed_view_ids_.size();

}

}  // namespace theia


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

#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>

int main() {
  Eigen::SparseMatrix<double> sm1(3, 3);
  /*
  sm1.reserve(3000000);

  {
    std::vector<Eigen::Triplet<double>> tripletList;
    tripletList.reserve(3000000);

    std::ifstream ifile ("output.txt");

    std::string substr;
    int i, j;
    double v;

    while (ifile) {
      std::getline(ifile, substr, ',');
      if (!ifile) break;
      i = std::stoi(substr);

      std::getline(ifile, substr, ',');
      j = std::stoi(substr);

      std::getline(ifile, substr);
      v = std::stod(substr);

      tripletList.push_back(Eigen::Triplet<double>(i, j, v));
    }

    sm1.setFromTriplets(tripletList.begin(), tripletList.end());
  }
  */

  std::vector<Eigen::Triplet<double>> tripletList = {
    {0, 0, 5},
    {1, 1, 3},
  };

  sm1.setFromTriplets(tripletList.begin(), tripletList.end());

  Eigen::SPQR<Eigen::SparseMatrix<double>> spqr(sm1);

  std::cout << "info : " << spqr.info() << std::endl;
  std::cout << "rank : " << spqr.rank() << std::endl;

  return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101111    E. Iorfida        File created.
 *      101111    E. Iorfida        Implementation of all the equations up to the Newton method.
 *      101117    E. Iorfida        Velocities computations added.
 *      101126    E. Iorfida        Get/set codes deleted.
 *      101206    E. Iorfida        LambertTargetingElements class deleted,
 *                                  added setInitialState, modified punctuation. Set single
 *                                  variables, change variables names in more understandable ones.
 *      101209    E. Iorfida        Corrected some coding errors.
 *      101213    E. Iorfida        Deleted lambertAngle, added numberOfRevolution, modified
 *                                  implementation.
 *      101214    E. Iorfida        Implementation only for the case with numberOfRevolution = 0.
 *      110113    E. Iorfida        Added necessary elements to build pointer-to-member-function
 *                                  to RootFinderAlgorithms and NewtonRaphsonMethod classes.
 *      110124    E. Iorfida        Added necessary piece of code to be able to use the last
 *                                  version of Newton-Raphson code.
 *      110126    E. Iorfida        Initialized member functions.
 *      110130    J. Melman         Simplified variable names, e.g., 'normOfdeletVelocityVector'
 *                                  became 'speed'. Requested references to specific formulas. Also
 *                                  corrected 'tangential' to 'transverse'. Simplified computation
 *                                  of radial unit vector. Corrected computation of transverse
 *                                  heliocentric velocity.
 *      110201    E. Iorfida        Added pointerToCelestialBody and modified variable names (from
 *                                  heliocentric, to inertial). Added patch for negative case of
 *                                  initialLambertGuess_. Added equations references.
 *      110206    E. Iorfida        Added unique function for Newton-Raphson method. Added
 *                                  computeAbsoluteValue to the initialLambertGuess_ for
 *                                  non-converging cases.
 *      110208    E. Iorfida        Added CartesianPositionElements objects as input and
 *                                  CartesianVelocityElements objects as output.
 *      110418    E. Iorfida        Added a new normal plane that take into account the case of two
 *                                  parallel position vector (with a relative angle of 180
 *                                  degrees). Better defined the pointers to the output
 *                                  CartesianVelocityElements.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120620    T. Secretin       Adapted and moved code from LambertTargeter.cpp.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *      140117    E. Brandon        Changed constructor input argument naming to be consistent with
 *                                  other Lambert classes.
 *
 *    References
 *
 *    Notes
 *
 */

#include <boost/make_shared.hpp>

#include <Eigen/Geometry>

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterGooding.h"

//! Tudat library namespace.
namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Constructor with immediate definition of parameters and execution of the algorithm.
LambertTargeterGooding::LambertTargeterGooding(
        const Eigen::Vector3d& aCartesianPositionAtDeparture,
        const Eigen::Vector3d& aCartesianPositionAtArrival,
        const double aTimeOfFlight,
        const double aGravitationalParameter,
        RootFinderPointer aRootFinder )
    : LambertTargeter( aCartesianPositionAtDeparture, aCartesianPositionAtArrival,
                       aTimeOfFlight, aGravitationalParameter ),
      rootFinder( aRootFinder )
{
    // Required because the make_shared in the function definition gives problems for MSVC.
    if ( !rootFinder.get( ) )
    {
        rootFinder = boost::make_shared< NewtonRaphson >( 1.0e-12, 1000 );
    }

    // Execute algorithm.
    execute( );
}

//! Execute Lambert targeting solver.
void LambertTargeterGooding::execute( )
{
    // Call Gooding's Lambert targeting routine.
    solveLambertProblemGooding( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                                timeOfFlight, gravitationalParameter,
                                cartesianVelocityAtDeparture, cartesianVelocityAtArrival,
                                rootFinder );
}

//! Get radial velocity at departure.
double LambertTargeterGooding::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterGooding::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterGooding::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterGooding::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival =
                angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterGooding::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat


//
// Created by joshua on 7/13/17.
//
#include "sim_env/Controller.h"
#include "sim_env/utils/EigenUtils.h"
#include <boost/math/constants/constants.hpp>
#include <cmath>

using namespace sim_env;

sim_env::PIDController::PIDController(float kp, float ki, float kd)
{
    _kp = kp;
    _ki = ki;
    _kd = kd;
    reset();
}

sim_env::PIDController::~PIDController()
{
    // nothing to do here
}

void sim_env::PIDController::setKp(float kp)
{
    _kp = kp;
}

void sim_env::PIDController::setKi(float ki)
{
    _ki = ki;
}

void sim_env::PIDController::setKd(float kd)
{
    _kd = kd;
}

void sim_env::PIDController::setGains(float kp, float ki, float kd)
{
    setKp(kp);
    setKi(ki);
    setKd(kd);
}

void sim_env::PIDController::setTarget(float target_state)
{
    if (target_state != _target) {
        reset();
        _target = target_state;
    }
}

float sim_env::PIDController::getTarget() const
{
    return _target;
}

bool sim_env::PIDController::isTargetSatisfied(float current_state, float threshold) const
{
    return std::fabs(current_state - _target) < threshold;
}

float sim_env::PIDController::control(float current_state)
{
    float error = _target - current_state;
    float delta_error = 0.0f;
    if (not std::isnan(_prev_error)) {
        delta_error = error - _prev_error;
    }
    _prev_error = error;
    _integral_part += error;
    float output = _kp * error + _ki * _integral_part + _kd * delta_error;
    //    if (isTargetSatisfied(current_state, 0.001f)) {
    //        auto logger = DefaultLogger::getInstance();
    //        std::stringstream ss;
    //        ss << "target is satisfied. output is " << output;
    //        ss << "error is " << error << " current state is " << current_state;
    //        ss << "kp: " << _kp << " ki" << _ki << "kd" << _kd;
    //        logger->logDebug(ss.str());
    //    }
    return output;
}

void sim_env::PIDController::reset()
{
    _integral_part = 0.0;
    _prev_error = nanf("");
}

////////////////////// IndependentMDPIDController //////////////////////////////
IndependentMDPIDController::IndependentMDPIDController(float kp, float ki, float kd)
{
    _default_ki = ki;
    _default_kp = kp;
    _default_kd = kd;
    setGains(kp, ki, kd);
}

IndependentMDPIDController::~IndependentMDPIDController()
{
    // nothing to do here.
}

void IndependentMDPIDController::setTarget(const Eigen::VectorXf& target_state)
{
    if (target_state.size() != _controllers.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setTarget]"
                                 "Invalid input state dimension.");
    }
    for (size_t i = 0; i < target_state.size(); ++i) {
        _controllers.at(i).setTarget(target_state[i]);
    }
}

void IndependentMDPIDController::getTarget(Eigen::VectorXf& target_state) const
{
    target_state.resize(_controllers.size());
    for (size_t i = 0; i < _controllers.size(); ++i) {
        target_state[i] = _controllers.at(i).getTarget();
    }
}

void IndependentMDPIDController::control(Eigen::VectorXf& output,
    const Eigen::VectorXf& current_state)
{
    if (_controllers.size() != current_state.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::control]"
                                 "Invalid input state dimension.");
    }
    output.resize(_controllers.size());
    for (size_t i = 0; i < _controllers.size(); ++i) {
        output[i] = _controllers.at(i).control(current_state[i]);
    }
}

void IndependentMDPIDController::reset()
{
    for (auto& controller : _controllers) {
        controller.reset();
    }
}

bool IndependentMDPIDController::isTargetSatisfied(Eigen::VectorXf& current_state, float threshold) const
{
    bool target_satisfied = true;
    for (size_t i = 0; i < _controllers.size(); ++i) {
        target_satisfied = target_satisfied and _controllers.at(i).isTargetSatisfied(current_state[i], threshold);
    }
    return target_satisfied;
}

void IndependentMDPIDController::setGains(float kp, float ki, float kd)
{
    for (auto& controller : _controllers) {
        controller.setGains(kp, ki, kd);
    }
}

void IndependentMDPIDController::setGains(const Eigen::VectorXf& kps, const Eigen::VectorXf& kis, const Eigen::VectorXf& kds)
{
    if (_controllers.size() != kps.size() or _controllers.size() != kis.size() or _controllers.size() != kds.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setGains]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setGains(kps[i], kis[i], kds[i]);
    }
}

void IndependentMDPIDController::setKps(const Eigen::VectorXf& kps)
{
    if (_controllers.size() != kps.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKps]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKp(kps[i]);
    }
}

void IndependentMDPIDController::setKp(float kp)
{
    for (auto& controller : _controllers) {
        controller.setKp(kp);
    }
}

void IndependentMDPIDController::setKis(const Eigen::VectorXf& kis)
{
    if (_controllers.size() != kis.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKis]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKi(kis[i]);
    }
}

void IndependentMDPIDController::setKi(float ki)
{
    for (auto& controller : _controllers) {
        controller.setKi(ki);
    }
}

void IndependentMDPIDController::setKds(const Eigen::VectorXf& kds)
{
    if (_controllers.size() != kds.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKds]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKd(kds[i]);
    }
}

void IndependentMDPIDController::setKd(float kd)
{
    for (auto& controller : _controllers) {
        controller.setKd(kd);
    }
}

unsigned int IndependentMDPIDController::getStateDimension()
{
    return (unsigned int)_controllers.size();
}

void IndependentMDPIDController::setStateDimension(unsigned int dim)
{
    if (dim != _controllers.size()) {
        unsigned int prev_dim = (unsigned int)_controllers.size();
        _controllers.resize(dim);
        reset();
        for (unsigned int i = prev_dim; i < _controllers.size(); ++i) {
            _controllers.at(i).setGains(_default_kp, _default_ki, _default_kd);
        }
    }
}

//*************************** RobotController ************************************//
RobotController::~RobotController() = default;

//*************************** RobotPositionController ****************************//
RobotPositionController::RobotPositionController(RobotPtr robot,
    RobotVelocityControllerPtr velocity_controller)
    : _pid_controller(1.0, 0.0, 0.0)
    , _velocity_controller(velocity_controller)
    , _robot(robot)
{
}

RobotPositionController::~RobotPositionController()
{
}

void RobotPositionController::setPositionProjectionFn(PositionProjectionFn pos_constraint)
{
    _pos_proj_fn = pos_constraint;
}

void RobotPositionController::setVelocityProjectionFn(VelocityProjectionFn vel_constraint)
{
    _vel_proj_fn = vel_constraint;
}

void RobotPositionController::setTarget(const Eigen::VectorXf& position)
{
    setTargetPosition(position);
}

void RobotPositionController::setTargetPosition(const Eigen::VectorXf& position)
{
    if (_robot.expired()) {
        LoggerPtr logger = DefaultLogger::getInstance();
        logger->logErr("Can not access underlying robot; the pointer is not valid anymore!",
            "[sim_env::RobotPositionController::setTargetPosition]");
    }
    RobotPtr robot = _robot.lock();
    LoggerPtr logger = robot->getWorld()->getLogger();
    // std::stringstream ss;
    // ss << "Setting target position " << position.transpose();
    // logger->logDebug(ss.str(), "[sim_env::RobotPositionController::setTargetPosition]");
    _pid_controller.setStateDimension((unsigned int)position.size());
    _pid_controller.setTarget(position);
}

unsigned int RobotPositionController::getTargetDimension() const
{
    auto robot = _robot.lock();
    return robot->getNumActiveDOFs();
}

RobotPtr RobotPositionController::getRobot() const
{
    return _robot.lock();
}

inline float cyclicPositionError(const Eigen::Array2f& pos_range, float pos, float target)
{
    float error = target - pos;
    float overflow_error = pos_range[1] - pos + target - pos_range[0];
    float underflow_error = pos_range[0] - pos + target - pos_range[1];
    error = std::abs(error) < std::abs(overflow_error) ? error : overflow_error;
    error = std::abs(error) < std::abs(underflow_error) ? error : underflow_error;
    return error;
}

bool RobotPositionController::control(const Eigen::VectorXf& positions, const Eigen::VectorXf& velocities,
    float timestep, RobotConstPtr robot,
    Eigen::VectorXf& output)
{
    Eigen::VectorXf target_position;
    _pid_controller.getTarget(target_position);
    if (target_position.size() != robot->getActiveDOFs().size()) {
        LoggerPtr logger = robot->getWorld()->getLogger();
        logger->logErr("The provided target position has different dimension from the active DOFs."
                       "[sim_env::RobotPositionController::setTargetPosition]");
        std::stringstream ss;
        ss << "target size is " << target_position.size() << " active dof size is " << robot->getNumActiveDOFs();
        logger->logErr(ss.str());
        return false;
    }
    // project target position onto constraint set (this should only do anything if the user set invalid target positions)
    if (_pos_proj_fn) {
        _pos_proj_fn(target_position, robot);
    }
    Eigen::VectorXf target_velocities(positions.size());
    //    _pid_controller.control(target_velocities, positions);
    // TODO see whether we still can use a PID somehow
    // TODO this is not moving in a straight line. we could make the decision on
    // TODO whether to move in a straight line or not dependent on a parameter
    // Eigen::ArrayX2f velocity_limits = robot->getDOFVelocityLimits();
    // Eigen::ArrayX2f acceleration_limits = robot->getDOFAccelerationLimits();
    // Eigen::VectorXf delta_position = target_position - positions;
    Eigen::VectorXi dof_indices = robot->getActiveDOFs();
    assert(dof_indices.size() == positions.size());
    DOFInformation dof_info;
    for (int idx = 0; idx < dof_indices.size(); ++idx) {
        // get dof information
        robot->getDOFInformation(dof_indices[idx], dof_info);
        float delta_position = target_position[idx] - positions[idx];
        if (dof_info.cyclic) {
            delta_position = cyclicPositionError(dof_info.position_limits, positions[idx], target_position[idx]);
        }
        // first, command max velocity for each dof separately
        float velocity_sign(1.0f);
        if (std::signbit(delta_position)) {
            // need negative velocity
            target_velocities[idx] = dof_info.velocity_limits[0];
            assert(std::signbit(target_velocities[idx]));
            velocity_sign = -1.0f;
        } else {
            // need positive velocity
            target_velocities[idx] = dof_info.velocity_limits[1];
            velocity_sign = 1.0f;
        }
        // next compute the maximum velocity we can have to not overshoot
        float abs_position_error = std::abs(delta_position);
        float abs_max_break_accel = 0.0f;
        if (std::signbit(velocities[idx])) {
            abs_max_break_accel = std::abs(dof_info.acceleration_limits[1]);
        } else {
            abs_max_break_accel = std::abs(dof_info.acceleration_limits[0]);
        }
        float abs_max_break_velocity = std::sqrt(2.0f * abs_position_error * abs_max_break_accel);
        // finally, set the target velocity such maximal, but such that we do not overshoot.
        target_velocities[idx] = velocity_sign * std::min(abs_max_break_velocity, std::abs(target_velocities[idx]));
    }
    // { // TODO delete this block

    //     auto robot = _robot.lock();
    //     auto world = robot->getWorld();
    //     auto logger = world->getLogger();
    //     logger->logDebug(boost::format("Position error: x:%1%, y: %2%, theta: %3%") % delta_position[0] % delta_position[1] % delta_position[2], "[sim_env::PositionController]");
    // }
    if (_vel_proj_fn) {
        _vel_proj_fn(target_velocities, robot);
    }
    _velocity_controller->setTargetVelocity(target_velocities);
    _velocity_controller->control(positions, velocities, timestep, robot, output);
    return true;
}

///////////////////////////// RobotVelocityController ///////////////////////////////
//RobotVelocityController::RobotVelocityController(RobotPtr robot):
//        _pid_controller(10.0, 0.0, 0.0),
//        _robot(robot) {
//}

RobotVelocityController::~RobotVelocityController() = default;

void RobotVelocityController::setTarget(const Eigen::VectorXf& target)
{
    setTargetVelocity(target);
}

//void RobotVelocityController::setTargetVelocity(const Eigen::VectorXf &velocity) {
//    if (_robot.expired()) {
//        LoggerPtr logger = DefaultLogger::getInstance();
//        logger->logErr("Can not access underlying robot; the pointer is not valid anymore!",
//                       "[sim_env::RobotVelocityController::setTargetVelocity]");
//    }
//    RobotPtr robot = _robot.lock();
//    Eigen::VectorXf new_target = velocity;
//    LoggerPtr logger = robot->getWorld()->getLogger();
//    // scale the target velocity to limits
//    using namespace utils::eigen;
//    ScalingResult scaling_result = scaleToLimits(new_target, robot->getDOFVelocityLimits());
//    if (scaling_result == ScalingResult::Failure) {
//        logger->logWarn("Impossible target velocity direction detected. The requested velocity can not be scaled to the given limits",
//                        "[sim_env::RobotVelocityController::setTargetVelocity]");
//    } else if (scaling_result == ScalingResult::Scaled) {
//        logger->logWarn("Requested velocity is out of limits. Scaling it down.",
//                        "[sim_env::RobotVelocityController::setTargetVelocity]");
//    }
//    std::stringstream ss;
//    ss << "Setting target velocity " << new_target.transpose();
//    logger->logDebug(ss.str(), "[sim_env::RobotVelocityController::setTargetVelocity]");
//    // set the new target
//    _pid_controller.setStateDimension((unsigned int) new_target.size());
//    _pid_controller.setTarget(new_target);
//}
//
//bool RobotVelocityController::control(const Eigen::VectorXf &positions, const Eigen::VectorXf &velocities,
//                                      float timestep, RobotConstPtr robot,
//                                      Eigen::VectorXf &output) {
//    Eigen::VectorXf target_velocity;
//    _pid_controller.getTarget(target_velocity);
//    if (target_velocity.size() != robot->getActiveDOFs().size()) {
//        LoggerPtr logger = robot->getWorld()->getLogger();
//        logger->logErr("The provided target position has different dimension from the active DOFs."
//                               "[sim_env::RobotVelocityController::setTargetVelocity]");
//        return false;
//    }
//    _pid_controller.control(output, velocities);
////    LoggerPtr logger = robot->getWorld()->getLogger();
////    std::stringstream ss;
////    ss << "Target velocities are " << target_velocity.transpose();
////    ss << " Current velocities are " << velocities.transpose();
////    logger->logDebug(ss.str());
//    // TODO limit efforts?
//    return true;
//}
//
//IndependentMDPIDController& RobotVelocityController::getPIDController() {
//   return _pid_controller;
//}
//
//LoggerPtr RobotVelocityController::getLogger() {
//    if (_robot.expired()) {
//        return DefaultLogger::getInstance();
//    }
//    RobotPtr robot = _robot.lock();
//    return robot->getWorld()->getLogger();
//}

SE2RobotPositionController::SE2RobotPositionController(RobotPtr robot, RobotVelocityControllerPtr velocity_controller)
    : _robot(robot)
    , _velocity_controller(velocity_controller)
{
    assert(robot->getNumDOFs() >= 3);
    assert(!robot->isStatic());
    // velocity limits
    auto dof_vel_limits = robot->getDOFVelocityLimits();
    float x_limit = std::min(std::abs(dof_vel_limits(0, 0)), std::abs(dof_vel_limits(0, 1)));
    float y_limit = std::min(std::abs(dof_vel_limits(1, 0)), std::abs(dof_vel_limits(1, 1)));
    _cartesian_vel_limit = std::min(x_limit, y_limit);
    _angular_vel_limit = std::min(std::abs(dof_vel_limits(2, 0)), std::abs(dof_vel_limits(2, 1)));
    // accelaration limits
    auto dof_acc_limits = robot->getDOFAccelerationLimits();
    x_limit = std::min(std::abs(dof_acc_limits(0, 0)), std::abs(dof_acc_limits(0, 1)));
    y_limit = std::min(std::abs(dof_acc_limits(1, 0)), std::abs(dof_acc_limits(1, 1)));
    _cartesian_acc_limit = std::min(x_limit, y_limit);
    _angular_acc_limit = std::min(std::abs(dof_acc_limits(2, 0)), std::abs(dof_acc_limits(2, 1)));
    _set_point.resize(3);
}

SE2RobotPositionController::~SE2RobotPositionController()
{
}

void SE2RobotPositionController::setPositionProjectionFn(PositionProjectionFn pos_constraint)
{
    _pos_proj_fn = pos_constraint;
}

void SE2RobotPositionController::setVelocityProjectionFn(VelocityProjectionFn vel_constraint)
{
    _vel_proj_fn = vel_constraint;
}

inline float normlizeOrientation(float v)
{
    const float pi = boost::math::constants::pi<float>();
    float sign = std::signbit(v) ? -1.0f : 1.0f;
    return sign * (std::abs(v) - std::floor((std::abs(v) + pi) / (2.0f * pi)) * 2.0f * pi);
}

inline float shortestSO2Direction(float val_1, float val_2)
{
    float value = val_2 - val_1;
    if (std::abs(value) > boost::math::constants::pi<float>()) {
        if (value > 0.0f) { // val_2 > val_1
            value -= 2.0f * boost::math::constants::pi<float>();
        } else {
            value += 2.0f * boost::math::constants::pi<float>();
        }
    }
    return value;
}

void SE2RobotPositionController::setTarget(const Eigen::VectorXf& target)
{
    static const std::string log_prefix("[SE2RobotPositionController::setTarget]");
    if (target.size() != 3) {
        auto robot = getRobot();
        auto logger = robot->getWorld()->getLogger();
        logger->logErr(boost::format("Target has invalid dimension. Expected dimension is 3, actual dimension is %1%") % target.size(), log_prefix);
        return;
    }
    _last_target = target;
    // normalize target orientation
    _last_target[2] = normlizeOrientation(_last_target[2]);

    // logger->logDebug(boost::format("Target %1%, %2%, %3%") % _last_target[0] % _last_target[1] % _last_target[2], log_prefix);
}

void SE2RobotPositionController::setTargetPosition(const Eigen::VectorXf& position)
{
    setTarget(position);
}

unsigned int SE2RobotPositionController::getTargetDimension() const
{
    return 3;
}

RobotPtr SE2RobotPositionController::getRobot() const
{
    auto robot = _robot.lock();
    if (!robot) {
        std::logic_error("[SE2RobotPositionController::getRobot] Could not acquire robot. Weak pointer expired.");
    }
    return robot;
}

bool SE2RobotPositionController::control(const Eigen::VectorXf& positions,
    const Eigen::VectorXf& velocities,
    float timestep,
    RobotConstPtr robot,
    Eigen::VectorXf& output)
{
    static const std::string log_prefix("[SE2RobotPositionController::control]");
    auto logger = robot->getConstWorld()->getConstLogger();
    if (_last_target.size() != 3) {
        logger->logWarn("No target set. Can not control any position", log_prefix);
        return false;
    }
    assert(positions.size() >= 3);
    // compute error in cartesian position
    Eigen::Vector2f cart_error = _last_target.head(2) - positions.head(2);
    // logger->logDebug(boost::format("Target %1%, %2%, %3%") % _last_target[0] % _last_target[1] % _last_target[2], log_prefix);
    float cart_error_norm = cart_error.norm();
    if (cart_error_norm > 0.0f) {
        cart_error /= cart_error_norm;
    }
    // compute the maximum velocity we can have to not overshoot
    float abs_max_break_velocity = std::sqrt(2.0f * cart_error_norm * _cartesian_acc_limit);
    float cart_vel = std::min(abs_max_break_velocity, _cartesian_vel_limit);
    // do the same for angular velocity
    // float angular_error = _last_target[2] - positions[2];
    float angular_error = shortestSO2Direction(positions[2], _last_target[2]);
    float angular_error_norm = std::abs(angular_error);
    float max_break_velocity_angular = std::sqrt(2.0f * angular_error_norm * _angular_acc_limit);
    float omega = std::min(max_break_velocity_angular, _angular_vel_limit);
    // if we are at our target position, command zero velocity, else compute a velocity taking us to our target
    if (angular_error_norm < 1e-4 and cart_error_norm < 1e-4) {
        _set_point.setZero();
    } else {
        // debug log
        // logger->logDebug(boost::format("cart_error: %1%, cart_vel: %2%, angular_error: %3%, angular_vel: %4%") % cart_error_norm % cart_vel % angular_error % omega, log_prefix);
        // now compute how much time we will need for each to reach its destination
        // we travel for some time at max velocity, followed by a decelaration phase
        // in the decelaration we slow down from v to 0, i.e. its duration is v / a. The distance we travel
        // in the decelaration is 0.5f * v / a. Accordingly, the time we are at the max velocity is t = s / v - 0.5 * v / a.
        // If we are in the deceleration phase already, t becomes negative.
        float t_angular_plateau = 0.0f;
        if (angular_error_norm > 0.0f) {
            t_angular_plateau = angular_error_norm / omega - 0.5f * omega / _angular_acc_limit;
        }
        float t_cart_plateau = 0.0f;
        if (cart_error_norm > 0.0f) {
            t_cart_plateau = cart_error_norm / cart_vel - 0.5f * cart_vel / _cartesian_acc_limit;
        }
        float t_angular = std::max(t_angular_plateau, 0.0f) + omega / _angular_acc_limit;
        float t_cart = std::max(t_cart_plateau, 0.0f) + cart_vel / _cartesian_acc_limit;
        // logger->logDebug(boost::format("Estimated duration till target, cart: %1%, angular %2%") % t_cart % t_angular, log_prefix);
        if (t_cart < t_angular) { // we will take more time to reach our angle destination, slow down cartesian
            float a = _cartesian_acc_limit * t_angular;
            float b = _cartesian_acc_limit * _cartesian_acc_limit * t_angular * t_angular - 2.0f * _cartesian_acc_limit * cart_error_norm;
            assert(b >= 0.0f);
            cart_vel = a - std::sqrt(b);
        } else { // we will take more time to reach our cartesian destination, slow down angular
            float a = _angular_acc_limit * t_cart;
            float b = _angular_acc_limit * _angular_acc_limit * t_cart * t_cart - 2.0f * _angular_acc_limit * angular_error_norm;
            assert(b >= 0.0f);
            omega = a - std::sqrt(b);
        }
        // logger->logDebug(boost::format("Commanded velocities: cart: %1%, angular: %2%") % cart_vel % omega, log_prefix);
        // logger->logDebug(boost::format("Cartesian velocity direction (x, y): %1%, %2%") % cart_error[0] % cart_error[1], log_prefix);
        _set_point.head(2) = cart_vel * cart_error;
        _set_point[2] = omega * angular_error / angular_error_norm;
        // TODO We should also define the velocities during acceleration phase. For large position errors and max velocities,
        // TODO it takes the robot different amount of time to accelerate to the respective target velocity for each DoF.
        // TODO Hence, the robot will not move all the time in the direction that we command (this depends on the underlying velocity controller of course).
        // TODO Alternatively, could define a velocity controller that operates in the same way as this position controller, i.e.
        // TODO one that ensures that a commanded target velocity is reached synchronously for all DOFs.
    }
    _velocity_controller->setTargetVelocity(_set_point);
    _velocity_controller->control(positions, velocities, timestep, robot, output);
    return true;
}


#pragma once

#include <csv2/writer.hpp>

#include <fmt/core.h>

#include <boost/math/special_functions/sign.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/numeric.hpp>

#include <cassert>
#include <cmath>
#include <iostream>
#include <limits>
#include <numeric>
#include <ranges>
#include <tuple>
#include <type_traits>

namespace profitview::util
{

class CsvWriter : public csv2::Writer<csv2::delimiter<','>>
{    // Provides shorthand
     // `write()`
public:
    CsvWriter(auto& stream)
        : csv2::Writer<csv2::delimiter<','>>(stream)
    {}

    void write(auto&&... args)
    {
        write_row<std::vector<std::string>>({fmt::format("{}", std::forward<decltype(args)>(args))...});
    }
};

// boost::accumulate should do this, but there's a compile fail
inline auto accumulate(const auto& s, auto i) -> auto { return std::accumulate(std::begin(s), std::end(s), i); }

inline auto accumulate(auto& s, auto i, auto op) -> auto { return std::accumulate(std::begin(s), std::end(s), i, op); }

auto stdev(auto& s, auto m, int p) -> auto
{    // Calculate standard deviation given mean (m)
    auto const variance{[&m, &p](auto a, const auto& v) { return a + (v - m) * (v - m) / (p - 1);}};

    return std::sqrt(accumulate(s, 0.0, variance));
}

auto ma(auto const& s, int p = 0) -> auto { return accumulate(s, 0.0) / (p ? p : s.size()); }

// Exponential Moving Average (ema) difference formula from
// https://en.wikipedia.org/wiki/Moving_average:
auto ema(auto const& s, auto p, auto m = 0) -> auto
{
    auto alpha{2.0f / (p - 1)};
    auto const ema_step{[&alpha](auto a, const auto& price) { return price * alpha + a * (1 - alpha); }};

    return accumulate(s, m, ema_step);
}

auto abs_differences(const auto& prices, int e) -> auto
{
    auto b{prices.end() - e};
    assert(prices.size() > e);

    using namespace std::ranges;

    subrange lagged{b - 1, prices.end() - 1}, aligned{b, prices.end()};
    std::vector<typename std::remove_cvref_t<decltype(prices)>::value_type> differences(e);
    transform(
        lagged, aligned, differences.begin(), [](auto n, auto m) -> auto { return std::abs(n - m); });

    return std::make_tuple(differences, prices.back() - *b);
}

auto is_monotonic(auto const& s) -> std::tuple<bool, bool>    // { monotonic, up }
{                                                             // If monotonic     ascending      { true,  true  }
    // If monotonic     descending     { true,  false }
    // If non-monotonic now ascending  { false, true  }
    // If non-monotonic now descending { false, false }
    auto sgn = [](int a, int b) { return a < b ? 1 : (a == b) ? 0 : -1; };
    int prev = 0;
    for (int i = 0; i < s.size() - 1; ++i)
    {
        int c = sgn(s[i], s[i + 1]);
        if (c != 0)
        {
            if (c != prev && prev != 0)
            {
                return {false, c == 1};
            }
            prev = c;
        }
    }
    return {true, prev == 1};
}

}    // namespace profitview::util