///////////////////////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_MP_TOMMATH_BACKEND_HPP
#define BOOST_MATH_MP_TOMMATH_BACKEND_HPP

#include <boost/multiprecision/number.hpp>
#include <boost/multiprecision/rational_adaptor.hpp>
#include <boost/multiprecision/detail/integer_ops.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/cstdint.hpp>
#include <boost/scoped_array.hpp>
#include <tommath.h>
#include <cmath>
#include <limits>
#include <climits>

namespace boost{ namespace multiprecision{ namespace backends{

namespace detail{

inline void check_tommath_result(unsigned v)
{
   if(v != MP_OKAY)
   {
      BOOST_THROW_EXCEPTION(std::runtime_error(mp_error_to_string(v)));
   }
}

}

struct tommath_int;

void eval_multiply(tommath_int& t, const tommath_int& o);
void eval_add(tommath_int& t, const tommath_int& o);

struct tommath_int
{
   typedef mpl::list<boost::int32_t, boost::long_long_type>             signed_types;
   typedef mpl::list<boost::uint32_t, boost::ulong_long_type>   unsigned_types;
   typedef mpl::list<long double>                           float_types;

   tommath_int()
   {
      detail::check_tommath_result(mp_init(&m_data));
   }
   tommath_int(const tommath_int& o)
   {
      detail::check_tommath_result(mp_init_copy(&m_data, const_cast< ::mp_int*>(&o.m_data)));
   }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   tommath_int(tommath_int&& o) BOOST_NOEXCEPT
   {
      m_data = o.m_data;
      o.m_data.dp = 0;
   }
   tommath_int& operator = (tommath_int&& o)
   {
      mp_exch(&m_data, &o.m_data);
      return *this;
   }
#endif
   tommath_int& operator = (const tommath_int& o)
   {
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      if(o.m_data.dp)
         detail::check_tommath_result(mp_copy(const_cast< ::mp_int*>(&o.m_data), &m_data));
      return *this;
   }
   tommath_int& operator = (boost::ulong_long_type i)
   {
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      boost::ulong_long_type mask = ((1uLL << std::numeric_limits<unsigned>::digits) - 1);
      unsigned shift = 0;
      ::mp_int t;
      detail::check_tommath_result(mp_init(&t));
      mp_zero(&m_data);
      while(i)
      {
         detail::check_tommath_result(mp_set_int(&t, static_cast<unsigned>(i & mask)));
         if(shift)
            detail::check_tommath_result(mp_mul_2d(&t, shift, &t));
         detail::check_tommath_result((mp_add(&m_data, &t, &m_data)));
         shift += std::numeric_limits<unsigned>::digits;
         i >>= std::numeric_limits<unsigned>::digits;
      }
      mp_clear(&t);
      return *this;
   }
   tommath_int& operator = (boost::long_long_type i)
   {
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      bool neg = i < 0;
      *this = boost::multiprecision::detail::unsigned_abs(i);
      if(neg)
         detail::check_tommath_result(mp_neg(&m_data, &m_data));
      return *this;
   }
   //
   // Note that although mp_set_int takes an unsigned long as an argument
   // it only sets the first 32-bits to the result, and ignores the rest.
   // So use uint32_t as the largest type to pass to this function.
   //
   tommath_int& operator = (boost::uint32_t i)
   {
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      detail::check_tommath_result((mp_set_int(&m_data, i)));
      return *this;
   }
   tommath_int& operator = (boost::int32_t i)
   {
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      bool neg = i < 0;
      *this = boost::multiprecision::detail::unsigned_abs(i);
      if(neg)
         detail::check_tommath_result(mp_neg(&m_data, &m_data));
      return *this;
   }
   tommath_int& operator = (long double a)
   {
      using std::frexp;
      using std::ldexp;
      using std::floor;

      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));

      if (a == 0) {
         detail::check_tommath_result(mp_set_int(&m_data, 0));
         return *this;
      }

      if (a == 1) {
         detail::check_tommath_result(mp_set_int(&m_data, 1));
         return *this;
      }

      BOOST_ASSERT(!(boost::math::isinf)(a));
      BOOST_ASSERT(!(boost::math::isnan)(a));

      int e;
      long double f, term;
      detail::check_tommath_result(mp_set_int(&m_data, 0u));
      ::mp_int t;
      detail::check_tommath_result(mp_init(&t));

      f = frexp(a, &e);

      static const int shift = std::numeric_limits<int>::digits - 1;

      while(f)
      {
         // extract int sized bits from f:
         f = ldexp(f, shift);
         term = floor(f);
         e -= shift;
         detail::check_tommath_result(mp_mul_2d(&m_data, shift, &m_data));
         if(term > 0)
         {
            detail::check_tommath_result(mp_set_int(&t, static_cast<int>(term)));
            detail::check_tommath_result(mp_add(&m_data, &t, &m_data));
         }
         else
         {
            detail::check_tommath_result(mp_set_int(&t, static_cast<int>(-term)));
            detail::check_tommath_result(mp_sub(&m_data, &t, &m_data));
         }
         f -= term;
      }
      if(e > 0)
         detail::check_tommath_result(mp_mul_2d(&m_data, e, &m_data));
      else if(e < 0)
      {
         tommath_int t2;
         detail::check_tommath_result(mp_div_2d(&m_data, -e, &m_data, &t2.data()));
      }
      mp_clear(&t);
      return *this;
   }
   tommath_int& operator = (const char* s)
   {
      //
      // We don't use libtommath's own routine because it doesn't error check the input :-(
      //
      if(m_data.dp == 0)
         detail::check_tommath_result(mp_init(&m_data));
      std::size_t n = s ? std::strlen(s) : 0;
      *this = static_cast<boost::uint32_t>(0u);
      unsigned radix = 10;
      bool isneg = false;
      if(n && (*s == '-'))
      {
         --n;
         ++s;
         isneg = true;
      }
      if(n && (*s == '0'))
      {
         if((n > 1) && ((s[1] == 'x') || (s[1] == 'X')))
         {
            radix = 16;
            s +=2;
            n -= 2;
         }
         else
         {
            radix = 8;
            n -= 1;
         }
      }
      if(n)
      {
         if(radix == 8 || radix == 16)
         {
            unsigned shift = radix == 8 ? 3 : 4;
            unsigned block_count = DIGIT_BIT / shift;
            unsigned block_shift = shift * block_count;
            boost::ulong_long_type val, block;
            while(*s)
            {
               block = 0;
               for(unsigned i = 0; (i < block_count); ++i)
               {
                  if(*s >= '0' && *s <= '9')
                     val = *s - '0';
                  else if(*s >= 'a' && *s <= 'f')
                     val = 10 + *s - 'a';
                  else if(*s >= 'A' && *s <= 'F')
                     val = 10 + *s - 'A';
                  else
                     val = 400;
                  if(val > radix)
                  {
                     BOOST_THROW_EXCEPTION(std::runtime_error("Unexpected content found while parsing character string."));
                  }
                  block <<= shift;
                  block |= val;
                  if(!*++s)
                  {
                     // final shift is different:
                     block_shift = (i + 1) * shift;
                     break;
                  }
               }
               detail::check_tommath_result(mp_mul_2d(&data(), block_shift, &data()));
               if(data().used)
                  data().dp[0] |= block;
               else
                  *this = block;
            }
         }
         else
         {
            // Base 10, we extract blocks of size 10^9 at a time, that way
            // the number of multiplications is kept to a minimum:
            boost::uint32_t block_mult = 1000000000;
            while(*s)
            {
               boost::uint32_t block = 0;
               for(unsigned i = 0; i < 9; ++i)
               {
                  boost::uint32_t val;
                  if(*s >= '0' && *s <= '9')
                     val = *s - '0';
                  else
                     BOOST_THROW_EXCEPTION(std::runtime_error("Unexpected character encountered in input."));
                  block *= 10;
                  block += val;
                  if(!*++s)
                  {
                     static const boost::uint32_t block_multiplier[9]  = { 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 };
                     block_mult = block_multiplier[i];
                     break;
                  }
               }
               tommath_int t;
               t = block_mult;
               eval_multiply(*this, t);
               t = block;
               eval_add(*this, t);
            }
         }
      }
      if(isneg)
         this->negate();
      return *this;
   }
   std::string str(std::streamsize /*digits*/, std::ios_base::fmtflags f)const
   {
      BOOST_ASSERT(m_data.dp);
      int base = 10;
      if((f & std::ios_base::oct) == std::ios_base::oct)
         base = 8;
      else if((f & std::ios_base::hex) == std::ios_base::hex)
         base = 16;
      //
      // sanity check, bases 8 and 16 are only available for positive numbers:
      //
      if((base != 10) && m_data.sign)
         BOOST_THROW_EXCEPTION(std::runtime_error("Formatted output in bases 8 or 16 is only available for positive numbers"));
      int s;
      detail::check_tommath_result(mp_radix_size(const_cast< ::mp_int*>(&m_data), base, &s));
      boost::scoped_array<char> a(new char[s+1]);
      detail::check_tommath_result(mp_toradix_n(const_cast< ::mp_int*>(&m_data), a.get(), base, s+1));
      std::string result = a.get();
      if((base != 10) && (f & std::ios_base::showbase))
      {
         int pos = result[0] == '-' ? 1 : 0;
         const char* pp = base == 8 ? "0" : "0x";
         result.insert(static_cast<std::string::size_type>(pos), pp);
      }
      if((f & std::ios_base::showpos) && (result[0] != '-'))
         result.insert(static_cast<std::string::size_type>(0), 1, '+');
      return result;
   }
   ~tommath_int()
   {
      if(m_data.dp)
         mp_clear(&m_data);
   }
   void negate()
   {
      BOOST_ASSERT(m_data.dp);
      mp_neg(&m_data, &m_data);
   }
   int compare(const tommath_int& o)const
   {
      BOOST_ASSERT(m_data.dp && o.m_data.dp);
      return mp_cmp(const_cast< ::mp_int*>(&m_data), const_cast< ::mp_int*>(&o.m_data));
   }
   template <class V>
   int compare(V v)const
   {
      tommath_int d;
      tommath_int t(*this);
      detail::check_tommath_result(mp_shrink(&t.data()));
      d = v;
      return t.compare(d);
   }
   ::mp_int& data()
   {
      BOOST_ASSERT(m_data.dp);
      return m_data;
   }
   const ::mp_int& data()const
   {
      BOOST_ASSERT(m_data.dp);
      return m_data;
   }
   void swap(tommath_int& o)BOOST_NOEXCEPT
   {
      mp_exch(&m_data, &o.data());
   }
protected:
   ::mp_int m_data;
};

#define BOOST_MP_TOMMATH_BIT_OP_CHECK(x)\
   if(SIGN(&x.data()))\
      BOOST_THROW_EXCEPTION(std::runtime_error("Bitwise operations on libtommath negative valued integers are disabled as they produce unpredictable results"))

int eval_get_sign(const tommath_int& val);

inline void eval_add(tommath_int& t, const tommath_int& o)
{
   detail::check_tommath_result(mp_add(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_subtract(tommath_int& t, const tommath_int& o)
{
   detail::check_tommath_result(mp_sub(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_multiply(tommath_int& t, const tommath_int& o)
{
   detail::check_tommath_result(mp_mul(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_divide(tommath_int& t, const tommath_int& o)
{
   using default_ops::eval_is_zero;
   tommath_int temp;
   if(eval_is_zero(o))
      BOOST_THROW_EXCEPTION(std::overflow_error("Integer division by zero"));
   detail::check_tommath_result(mp_div(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data(), &temp.data()));
}
inline void eval_modulus(tommath_int& t, const tommath_int& o)
{
   using default_ops::eval_is_zero;
   if(eval_is_zero(o))
      BOOST_THROW_EXCEPTION(std::overflow_error("Integer division by zero"));
   bool neg = eval_get_sign(t) < 0;
   bool neg2 = eval_get_sign(o) < 0;
   detail::check_tommath_result(mp_mod(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
   if((neg != neg2) && (eval_get_sign(t) != 0))
   {
      t.negate();
      detail::check_tommath_result(mp_add(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
      t.negate();
   }
   else if(neg && (t.compare(o) == 0))
   {
      mp_zero(&t.data());
   }
}
template <class UI>
inline void eval_left_shift(tommath_int& t, UI i)
{
   detail::check_tommath_result(mp_mul_2d(&t.data(), static_cast<unsigned>(i), &t.data()));
}
template <class UI>
inline void eval_right_shift(tommath_int& t, UI i)
{
   tommath_int d;
   detail::check_tommath_result(mp_div_2d(&t.data(), static_cast<unsigned>(i), &t.data(), &d.data()));
}
template <class UI>
inline void eval_left_shift(tommath_int& t, const tommath_int& v, UI i)
{
   detail::check_tommath_result(mp_mul_2d(const_cast< ::mp_int*>(&v.data()), static_cast<unsigned>(i), &t.data()));
}
template <class UI>
inline void eval_right_shift(tommath_int& t, const tommath_int& v, UI i)
{
   tommath_int d;
   detail::check_tommath_result(mp_div_2d(const_cast< ::mp_int*>(&v.data()), static_cast<unsigned long>(i), &t.data(), &d.data()));
}

inline void eval_bitwise_and(tommath_int& result, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(result);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_and(&result.data(), const_cast< ::mp_int*>(&v.data()), &result.data()));
}

inline void eval_bitwise_or(tommath_int& result, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(result);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_or(&result.data(), const_cast< ::mp_int*>(&v.data()), &result.data()));
}

inline void eval_bitwise_xor(tommath_int& result, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(result);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_xor(&result.data(), const_cast< ::mp_int*>(&v.data()), &result.data()));
}

inline void eval_add(tommath_int& t, const tommath_int& p, const tommath_int& o)
{
   detail::check_tommath_result(mp_add(const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_subtract(tommath_int& t, const tommath_int& p, const tommath_int& o)
{
   detail::check_tommath_result(mp_sub(const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_multiply(tommath_int& t, const tommath_int& p, const tommath_int& o)
{
   detail::check_tommath_result(mp_mul(const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&o.data()), &t.data()));
}
inline void eval_divide(tommath_int& t, const tommath_int& p, const tommath_int& o)
{
   using default_ops::eval_is_zero;
   tommath_int d;
   if(eval_is_zero(o))
      BOOST_THROW_EXCEPTION(std::overflow_error("Integer division by zero"));
   detail::check_tommath_result(mp_div(const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&o.data()), &t.data(), &d.data()));
}
inline void eval_modulus(tommath_int& t, const tommath_int& p, const tommath_int& o)
{
   using default_ops::eval_is_zero;
   if(eval_is_zero(o))
      BOOST_THROW_EXCEPTION(std::overflow_error("Integer division by zero"));
   bool neg = eval_get_sign(p) < 0;
   bool neg2 = eval_get_sign(o) < 0;
   detail::check_tommath_result(mp_mod(const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&o.data()), &t.data()));
   if((neg != neg2) && (eval_get_sign(t) != 0))
   {
      t.negate();
      detail::check_tommath_result(mp_add(&t.data(), const_cast< ::mp_int*>(&o.data()), &t.data()));
      t.negate();
   }
   else if(neg  && (t.compare(o) == 0))
   {
      mp_zero(&t.data());
   }
}

inline void eval_bitwise_and(tommath_int& result, const tommath_int& u, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(u);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_and(const_cast< ::mp_int*>(&u.data()), const_cast< ::mp_int*>(&v.data()), &result.data()));
}

inline void eval_bitwise_or(tommath_int& result, const tommath_int& u, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(u);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_or(const_cast< ::mp_int*>(&u.data()), const_cast< ::mp_int*>(&v.data()), &result.data()));
}

inline void eval_bitwise_xor(tommath_int& result, const tommath_int& u, const tommath_int& v)
{
   BOOST_MP_TOMMATH_BIT_OP_CHECK(u);
   BOOST_MP_TOMMATH_BIT_OP_CHECK(v);
   detail::check_tommath_result(mp_xor(const_cast< ::mp_int*>(&u.data()), const_cast< ::mp_int*>(&v.data()), &result.data()));
}
/*
inline void eval_complement(tommath_int& result, const tommath_int& u)
{
   //
   // Although this code works, it doesn't really do what the user might expect....
   // and it's hard to see how it ever could.  Disabled for now:
   //
   result = u;
   for(int i = 0; i < result.data().used; ++i)
   {
      result.data().dp[i] = MP_MASK & ~(result.data().dp[i]);
   }
   //
   // We now need to pad out the left of the value with 1's to round up to a whole number of
   // CHAR_BIT * sizeof(mp_digit) units.  Otherwise we'll end up with a very strange number of
   // bits set!
   //
   unsigned shift = result.data().used * DIGIT_BIT;    // How many bits we're actually using
   // How many bits we actually need, reduced by one to account for a mythical sign bit:
   int padding = result.data().used * std::numeric_limits<mp_digit>::digits - shift - 1;
   while(padding >= std::numeric_limits<mp_digit>::digits)
      padding -= std::numeric_limits<mp_digit>::digits;

   // Create a mask providing the extra bits we need and add to result:
   tommath_int mask;
   mask = static_cast<boost::long_long_type>((1u << padding) - 1);
   eval_left_shift(mask, shift);
   add(result, mask);
}
*/
inline bool eval_is_zero(const tommath_int& val)
{
   return mp_iszero(&val.data());
}
inline int eval_get_sign(const tommath_int& val)
{
   return mp_iszero(&val.data()) ? 0 : SIGN(&val.data()) ? -1 : 1;
}
template <class A>
inline void eval_convert_to(A* result, const tommath_int& val)
{
   *result = boost::lexical_cast<A>(val.str(0, std::ios_base::fmtflags(0)));
}
inline void eval_convert_to(char* result, const tommath_int& val)
{
   *result = static_cast<char>(boost::lexical_cast<int>(val.str(0, std::ios_base::fmtflags(0))));
}
inline void eval_convert_to(unsigned char* result, const tommath_int& val)
{
   *result = static_cast<unsigned char>(boost::lexical_cast<unsigned>(val.str(0, std::ios_base::fmtflags(0))));
}
inline void eval_convert_to(signed char* result, const tommath_int& val)
{
   *result = static_cast<signed char>(boost::lexical_cast<int>(val.str(0, std::ios_base::fmtflags(0))));
}
inline void eval_abs(tommath_int& result, const tommath_int& val)
{
   detail::check_tommath_result(mp_abs(const_cast< ::mp_int*>(&val.data()), &result.data()));
}
inline void eval_gcd(tommath_int& result, const tommath_int& a, const tommath_int& b)
{
   detail::check_tommath_result(mp_gcd(const_cast< ::mp_int*>(&a.data()), const_cast< ::mp_int*>(&b.data()), const_cast< ::mp_int*>(&result.data())));
}
inline void eval_lcm(tommath_int& result, const tommath_int& a, const tommath_int& b)
{
   detail::check_tommath_result(mp_lcm(const_cast< ::mp_int*>(&a.data()), const_cast< ::mp_int*>(&b.data()), const_cast< ::mp_int*>(&result.data())));
}
inline void eval_powm(tommath_int& result, const tommath_int& base, const tommath_int& p, const tommath_int& m)
{
   if(eval_get_sign(p) < 0)
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("powm requires a positive exponent."));
   }
   detail::check_tommath_result(mp_exptmod(const_cast< ::mp_int*>(&base.data()), const_cast< ::mp_int*>(&p.data()), const_cast< ::mp_int*>(&m.data()), &result.data()));
}

inline void eval_qr(const tommath_int& x, const tommath_int& y,
   tommath_int& q, tommath_int& r)
{
   detail::check_tommath_result(mp_div(const_cast< ::mp_int*>(&x.data()), const_cast< ::mp_int*>(&y.data()), &q.data(), &r.data()));
}

inline unsigned eval_lsb(const tommath_int& val)
{
   int c = eval_get_sign(val);
   if(c == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(c < 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }
   return mp_cnt_lsb(const_cast< ::mp_int*>(&val.data()));
}

inline unsigned eval_msb(const tommath_int& val)
{
   int c = eval_get_sign(val);
   if(c == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(c < 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }
   return mp_count_bits(const_cast< ::mp_int*>(&val.data())) - 1;
}

template <class Integer>
inline typename enable_if<is_unsigned<Integer>, Integer>::type eval_integer_modulus(const tommath_int& x, Integer val)
{
   static const mp_digit m = (static_cast<mp_digit>(1) << DIGIT_BIT) - 1;
   if(val <= m)
   {
      mp_digit d;
      detail::check_tommath_result(mp_mod_d(const_cast< ::mp_int*>(&x.data()), static_cast<mp_digit>(val), &d));
      return d;
   }
   else
   {
      return default_ops::eval_integer_modulus(x, val);
   }
}
template <class Integer>
inline typename enable_if<is_signed<Integer>, Integer>::type eval_integer_modulus(const tommath_int& x, Integer val)
{
   return eval_integer_modulus(x, boost::multiprecision::detail::unsigned_abs(val));
}

} // namespace backends

using boost::multiprecision::backends::tommath_int;

template<>
struct number_category<tommath_int> : public mpl::int_<number_kind_integer>{};

typedef number<tommath_int >                     tom_int;
typedef rational_adaptor<tommath_int>               tommath_rational;
typedef number<tommath_rational>                 tom_rational;

}}  // namespaces

namespace std{

template<boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >
{
   typedef boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> number_type;
public:
   BOOST_STATIC_CONSTEXPR bool is_specialized = true;
   //
   // Largest and smallest numbers are bounded only by available memory, set
   // to zero:
   //
   static number_type (min)()
   {
      return number_type();
   }
   static number_type (max)()
   {
      return number_type();
   }
   static number_type lowest() { return (min)(); }
   BOOST_STATIC_CONSTEXPR int digits = INT_MAX;
   BOOST_STATIC_CONSTEXPR int digits10 = (INT_MAX / 1000) * 301L;
   BOOST_STATIC_CONSTEXPR int max_digits10 = digits10 + 2;
   BOOST_STATIC_CONSTEXPR bool is_signed = true;
   BOOST_STATIC_CONSTEXPR bool is_integer = true;
   BOOST_STATIC_CONSTEXPR bool is_exact = true;
   BOOST_STATIC_CONSTEXPR int radix = 2;
   static number_type epsilon() { return number_type(); }
   static number_type round_error() { return number_type(); }
   BOOST_STATIC_CONSTEXPR int min_exponent = 0;
   BOOST_STATIC_CONSTEXPR int min_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR bool has_infinity = false;
   BOOST_STATIC_CONSTEXPR bool has_quiet_NaN = false;
   BOOST_STATIC_CONSTEXPR bool has_signaling_NaN = false;
   BOOST_STATIC_CONSTEXPR float_denorm_style has_denorm = denorm_absent;
   BOOST_STATIC_CONSTEXPR bool has_denorm_loss = false;
   static number_type infinity() { return number_type(); }
   static number_type quiet_NaN() { return number_type(); }
   static number_type signaling_NaN() { return number_type(); }
   static number_type denorm_min() { return number_type(); }
   BOOST_STATIC_CONSTEXPR bool is_iec559 = false;
   BOOST_STATIC_CONSTEXPR bool is_bounded = false;
   BOOST_STATIC_CONSTEXPR bool is_modulo = false;
   BOOST_STATIC_CONSTEXPR bool traps = false;
   BOOST_STATIC_CONSTEXPR bool tinyness_before = false;
   BOOST_STATIC_CONSTEXPR float_round_style round_style = round_toward_zero;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::digits;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::digits10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::max_digits10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_signed;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_integer;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_exact;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::radix;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::min_exponent;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::min_exponent10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::max_exponent;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::max_exponent10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::has_infinity;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::has_quiet_NaN;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::has_signaling_NaN;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST float_denorm_style numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::has_denorm;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::has_denorm_loss;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_iec559;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_bounded;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::is_modulo;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::traps;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::tinyness_before;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST float_round_style numeric_limits<boost::multiprecision::number<boost::multiprecision::tommath_int, ExpressionTemplates> >::round_style;

#endif
}

#endif


// Author: Francesco Regazzoni - MOX, Politecnico di Milano
// Email:  francesco.regazzoni@polimi.it
// Date:   2020

#include <cmath>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "model_RDQ20_SE.hpp"

model_RDQ20_SE::model_RDQ20_SE(std::string parameters_file)
    : sarcomere("RDQ20-SE") {
  // Read JSON options file
  boost::property_tree::ptree root;
  boost::property_tree::read_json(parameters_file, root);

  prm_n_RU = root.get_child("geometry")
                 .get_child("n_RU")
                 .get_value<unsigned int>(); // [-]
  prm_LA = root.get_child("geometry")
               .get_child("LA")
               .get_value<double>(); // [micro m]
  prm_LM = root.get_child("geometry")
               .get_child("LM")
               .get_value<double>(); // [micro m]
  prm_LB = root.get_child("geometry")
               .get_child("LB")
               .get_value<double>(); // [micro m]
  prm_SL0 = root.get_child("geometry")
                .get_child("SL0")
                .get_value<double>(); // [micro m]
  prm_Lsmooth = root.get_child("geometry")
                    .get_child("Lsmooth")
                    .get_value<double>(); // [micro m]
  prm_Q = root.get_child("RU_steady_state")
              .get_child("Q")
              .get_value<double>(); // [-]
  prm_Kd0 = root.get_child("RU_steady_state")
                .get_child("Kd0")
                .get_value<double>(); // [micro M]
  prm_alphaKd = root.get_child("RU_steady_state")
                    .get_child("alphaKd")
                    .get_value<double>(); // [micro M / micro m]
  prm_mu = root.get_child("RU_steady_state")
               .get_child("mu")
               .get_value<double>(); // [-]
  prm_gamma = root.get_child("RU_steady_state")
                  .get_child("gamma")
                  .get_value<double>(); // [-]
  prm_Koff = root.get_child("RU_kinetics")
                 .get_child("Koff")
                 .get_value<double>(); // [s^-1]
  prm_Kbasic = root.get_child("RU_kinetics")
                   .get_child("Kbasic")
                   .get_value<double>(); // [s^-1]
  prm_r0 = root.get_child("XB_cycling")
               .get_child("r0")
               .get_value<double>(); // [s^-1]
  prm_alpha = root.get_child("XB_cycling")
                  .get_child("alpha")
                  .get_value<double>(); // [-]
  prm_mu0_fP = root.get_child("XB_cycling")
                   .get_child("mu0_fP")
                   .get_value<double>(); // [s^-1]
  prm_mu1_fP = root.get_child("XB_cycling")
                   .get_child("mu1_fP")
                   .get_value<double>(); // [s^-1]
  prm_a_XB = root.get_child("upscaling")
                 .get_child("a_XB")
                 .get_value<double>(); // [kPa]

  allocate_variables();

  initialize_rates();
}

void model_RDQ20_SE::allocate_variables() {
  // Variable numbers
  n_states_RU = (prm_n_RU - 2) * 4 * 4 * 4;
  n_states_XB = prm_n_RU * 4;
  n_variables = n_states_RU + n_states_XB;

  // Allocation of state_RU
  std::array<std::array<std::array<double, 4>, 4>, 4> base_RU_state;
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        base_RU_state[RU_L][RU_C][RU_R] = 0.0;
  base_RU_state[0][0][0] = 1.0;

  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    state_RU.push_back(base_RU_state);

  // Allocation of state_XB
  std::array<double, 4> base_XB_state;
  for (i_XB = 0; i_XB < 4; ++i_XB)
    base_XB_state[i_XB] = 0.0;

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    state_XB.push_back(base_XB_state);

  // Allocation of initial_state
  for (unsigned int i = 0; i < n_variables; ++i)
    initial_state.push_back(0.0);
  serialize_state(initial_state);

  // Allocation of rates_RU
  std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>
      base_RU_rates_or_flux;
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new)
          base_RU_rates_or_flux[RU_L][RU_C][RU_R][RU_new] = 0.0;

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    rates_RU.push_back(base_RU_rates_or_flux);
  }

  // Allocation of flux_RU_*
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU) {
    flux_RU_L.push_back(base_RU_rates_or_flux);
    flux_RU_C.push_back(base_RU_rates_or_flux);
    flux_RU_R.push_back(base_RU_rates_or_flux);
  }
}

void model_RDQ20_SE::solve_time_step(std::vector<double> &state,
                                     const double &calcium,
                                     const double &sarcomere_length,
                                     const double &dSL_dt, const double &dt) {
  // Deserialize state
  deserialize_state(state);

  // Update RU transition rates
  RU_update_rates(calcium, sarcomere_length);

  // Advance RU state
  double RU_dt = 0.0;
  double time_advanced = 0.0;
  while (time_advanced <=
         dt - 1e-10) // Cover the time-step up to a given tolerance
  {
    RU_dt = std::min(prm_time_step_update_RU_state, dt - time_advanced);
    RU_update_state(RU_dt);
    time_advanced += RU_dt;
  }

  // Advance XB state
  XB_update_state(sarcomere_length, dSL_dt, dt);

  // Re-serialize state
  serialize_state(state);
}

void model_RDQ20_SE::deserialize_state(const std::vector<double> &state) {
  unsigned int i_current = 0;
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          state_RU[i_RU][RU_L][RU_C][RU_R] = state[i_current];
          i_current++;
        }

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    for (i_XB = 0; i_XB < 4; ++i_XB) {
      state_XB[i_RU][i_XB] = state[i_current];
      i_current++;
    }
}

void model_RDQ20_SE::serialize_state(std::vector<double> &state) {
  unsigned int i_current = 0;
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          state[i_current] = state_RU[i_RU][RU_L][RU_C][RU_R];
          i_current++;
        }

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    for (i_XB = 0; i_XB < 4; ++i_XB) {
      state[i_current] = state_XB[i_RU][i_XB];
      i_current++;
    }
}

double model_RDQ20_SE::get_active_tension(const std::vector<double> &state,
                                          const double & /*sarcomere_length*/) {
  deserialize_state(state);

  double active_tension = 0.0;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    active_tension += state_XB[i_RU][1] + state_XB[i_RU][3];

  return prm_a_XB * active_tension / prm_n_RU;
}

double
model_RDQ20_SE::get_active_stiffness(const std::vector<double> &state,
                                     const double & /*sarcomere_length*/) {
  deserialize_state(state);

  double active_stiffness = 0.0;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    active_stiffness += state_XB[i_RU][0] + state_XB[i_RU][2];

  return prm_a_XB * active_stiffness / prm_n_RU;
}

double model_RDQ20_SE::get_permissivity(const std::vector<double> &state) {
  deserialize_state(state);

  double permissivity = 0;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    if (i_RU == 0) // First RU
    {
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          permissivity +=
              state_RU[0][2][RU_C][RU_R] + state_RU[0][3][RU_C][RU_R];
    } else if (i_RU == prm_n_RU - 1) // Last RU
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_C = 0; RU_C < 4; ++RU_C)
          permissivity += state_RU[prm_n_RU - 3][RU_L][RU_C][2] +
                          state_RU[prm_n_RU - 3][RU_L][RU_C][3];
    } else // Intermediate RUs
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          permissivity += state_RU[i_RU - 1][RU_L][2][RU_R] +
                          state_RU[i_RU - 1][RU_L][3][RU_R];
    }
  }
  return permissivity / prm_n_RU;
}

void model_RDQ20_SE::initialize_rates() {
  int permissive_neighbors;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_R = 0; RU_R < 4; ++RU_R) {
        permissive_neighbors =
            permissivity_of_state[RU_L] + permissivity_of_state[RU_R];
        rates_RU[i_RU][RU_L][1][RU_R][0] = prm_Koff;
        rates_RU[i_RU][RU_L][2][RU_R][3] = prm_Koff / prm_mu;
        rates_RU[i_RU][RU_L][3][RU_R][0] =
            prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
        rates_RU[i_RU][RU_L][2][RU_R][1] =
            prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
      }
}

void model_RDQ20_SE::RU_update_rates(const double &calcium,
                                     const double &sarcomere_length) {
  double Kon = prm_Koff / (prm_Kd0 - prm_alphaKd * (2.15 - sarcomere_length));
  int permissive_neighbors;
  double ChiSF_i_RU;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    ChiSF_i_RU = ChiSF(sarcomere_length, i_RU);
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_R = 0; RU_R < 4; ++RU_R) {
        permissive_neighbors =
            permissivity_of_state[RU_L] + permissivity_of_state[RU_R];
        rates_RU[i_RU][RU_L][0][RU_R][1] = Kon * calcium;
        rates_RU[i_RU][RU_L][3][RU_R][2] = Kon * calcium;
        rates_RU[i_RU][RU_L][1][RU_R][2] =
            ChiSF_i_RU * std::pow(prm_gamma, permissive_neighbors) * prm_Q *
            prm_Kbasic;
        rates_RU[i_RU][RU_L][0][RU_R][3] =
            ChiSF_i_RU * std::pow(prm_gamma, permissive_neighbors) * prm_Q *
            prm_Kbasic / prm_mu;
      }
  }
}

void model_RDQ20_SE::RU_update_state(const double &dt) {
  // Compute fluxes associated with center units
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          for (RU_new = 0; RU_new < 4; ++RU_new) {
            flux_RU_C[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rates_RU[i_RU + 1][RU_L][RU_C][RU_R][RU_new] *
                state_RU[i_RU][RU_L][RU_C][RU_R];
          }

  double prob_tot;
  double rate_tot;

  // Compute fluxes associated with left units
  // --- Most left-ward triplet
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new) {
          flux_RU_L[0][RU_L][RU_C][RU_R][RU_new] =
              rates_RU[0][0][RU_L][RU_C][RU_new] *
              state_RU[0][RU_L][RU_C][RU_R];
        }

  // --- Other triplets
  for (i_RU = 1; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C) {
        prob_tot = 0.0;
        for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
          prob_tot += state_RU[i_RU - 1][RU_dummy][RU_L][RU_C];

        for (RU_new = 0; RU_new < 4; ++RU_new) {
          rate_tot = 0.0;
          if (prob_tot > 1e-12) {
            for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
              rate_tot += flux_RU_C[i_RU - 1][RU_dummy][RU_L][RU_C][RU_new];

            rate_tot /= prob_tot;
          }

          for (RU_R = 0; RU_R < 4; ++RU_R) {
            flux_RU_L[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rate_tot * state_RU[i_RU][RU_L][RU_C][RU_R];
          }
        }
      }

  // Compute fluxes associated with right units
  // --- Most right-ward triplet
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new) {
          flux_RU_R[prm_n_RU - 3][RU_L][RU_C][RU_R][RU_new] =
              rates_RU[prm_n_RU - 1][RU_C][RU_R][0][RU_new] *
              state_RU[prm_n_RU - 3][RU_L][RU_C][RU_R];
        }

  // --- Other triplets
  for (i_RU = 0; i_RU < prm_n_RU - 3; ++i_RU)
    for (RU_R = 0; RU_R < 4; ++RU_R)
      for (RU_C = 0; RU_C < 4; ++RU_C) {
        prob_tot = 0.0;
        for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
          prob_tot += state_RU[i_RU + 1][RU_C][RU_R][RU_dummy];

        for (RU_new = 0; RU_new < 4; ++RU_new) {
          rate_tot = 0.0;
          if (prob_tot > 1e-12) {
            for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
              rate_tot += flux_RU_C[i_RU + 1][RU_C][RU_R][RU_dummy][RU_new];

            rate_tot /= prob_tot;
          }

          for (RU_L = 0; RU_L < 4; ++RU_L) {
            flux_RU_R[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rate_tot * state_RU[i_RU][RU_L][RU_C][RU_R];
          }
        }
      }

  // Forward Euler advance
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          for (RU_new = 0; RU_new < 4; ++RU_new) {
            state_RU[i_RU][RU_L][RU_C][RU_R] +=
                dt * (flux_RU_L[i_RU][RU_new][RU_C][RU_R][RU_L] +
                      flux_RU_C[i_RU][RU_L][RU_new][RU_R][RU_C] +
                      flux_RU_R[i_RU][RU_L][RU_C][RU_new][RU_R] -
                      flux_RU_L[i_RU][RU_L][RU_C][RU_R][RU_new] -
                      flux_RU_C[i_RU][RU_L][RU_C][RU_R][RU_new] -
                      flux_RU_R[i_RU][RU_L][RU_C][RU_R][RU_new]);
          }
}

void model_RDQ20_SE::XB_update_state(const double &sarcomere_length,
                                     const double &dSL_dt, const double &dt) {
  double v = -dSL_dt / prm_SL0;

  double r = prm_r0 + prm_alpha * std::abs(v);
  double k_PN;
  double k_NP;
  double permissivity;
  double flux_PN;
  double flux_NP;
  double ChiSFChiMF_i;
  double diag_P;
  double diag_N;

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    permissivity = 0;
    flux_PN = 0;
    flux_NP = 0;

    if (i_RU == 0) // First RU
    {
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          permissivity +=
              state_RU[0][2][RU_C][RU_R] + state_RU[0][3][RU_C][RU_R];
          flux_NP += state_RU[0][0][RU_C][RU_R] * rates_RU[0][0][0][RU_C][3] +
                     state_RU[0][1][RU_C][RU_R] * rates_RU[0][0][1][RU_C][2];
          flux_PN += state_RU[0][3][RU_C][RU_R] * rates_RU[0][0][3][RU_C][0] +
                     state_RU[0][2][RU_C][RU_R] * rates_RU[0][0][2][RU_C][1];
        }
    } else if (i_RU == prm_n_RU - 1) // Last RU
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_C = 0; RU_C < 4; ++RU_C) {
          permissivity += state_RU[prm_n_RU - 3][RU_L][RU_C][2] +
                          state_RU[prm_n_RU - 3][RU_L][RU_C][3];
          flux_NP += state_RU[prm_n_RU - 3][RU_L][RU_C][0] *
                         rates_RU[prm_n_RU - 1][RU_C][0][0][3] +
                     state_RU[prm_n_RU - 3][RU_L][RU_C][1] *
                         rates_RU[prm_n_RU - 1][RU_C][1][0][2];
          flux_PN += state_RU[prm_n_RU - 3][RU_L][RU_C][3] *
                         rates_RU[prm_n_RU - 1][RU_C][3][0][0] +
                     state_RU[prm_n_RU - 3][RU_L][RU_C][2] *
                         rates_RU[prm_n_RU - 1][RU_C][2][0][1];
        }
    } else // Intermediate RUs
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          permissivity += state_RU[i_RU - 1][RU_L][2][RU_R] +
                          state_RU[i_RU - 1][RU_L][3][RU_R];
          flux_NP += state_RU[i_RU - 1][RU_L][0][RU_R] *
                         rates_RU[i_RU][RU_L][0][RU_R][3] +
                     state_RU[i_RU - 1][RU_L][1][RU_R] *
                         rates_RU[i_RU][RU_L][1][RU_R][2];
          flux_PN += state_RU[i_RU - 1][RU_L][3][RU_R] *
                         rates_RU[i_RU][RU_L][3][RU_R][0] +
                     state_RU[i_RU - 1][RU_L][2][RU_R] *
                         rates_RU[i_RU][RU_L][2][RU_R][1];
        }
    }

    if (permissivity >= 1e-12)
      k_PN = 0;
    else
      k_PN = flux_PN / permissivity;

    if (1 - permissivity >= 1e-12)
      k_NP = 0;
    else
      k_NP = flux_NP / (1 - permissivity);

    diag_P = r + k_PN;
    diag_N = r + k_NP;

    // Fill matrix
    XB_A(0, 0) = -diag_P;
    XB_A(1, 1) = -diag_P;
    XB_A(2, 2) = -diag_N;
    XB_A(3, 3) = -diag_N;
    XB_A(0, 2) = k_NP;
    XB_A(1, 3) = k_NP;
    XB_A(2, 0) = k_PN;
    XB_A(3, 1) = k_PN;
    XB_A(1, 0) = -v;
    XB_A(3, 2) = -v;

    XB_A *= -dt;
    for (i_XB = 0; i_XB < 4; ++i_XB)
      XB_A(i_XB, i_XB) += 1.0;

    ChiSFChiMF_i =
        ChiSF(sarcomere_length, i_RU) * ChiMF(sarcomere_length, i_RU);

    // Fill rhs
    XB_rhs(0) =
        state_XB[i_RU][0] + dt * permissivity * prm_mu0_fP * ChiSFChiMF_i;
    XB_rhs(1) =
        state_XB[i_RU][1] + dt * permissivity * prm_mu1_fP * ChiSFChiMF_i;
    XB_rhs(2) = state_XB[i_RU][2];
    XB_rhs(3) = state_XB[i_RU][3];

    // Implicit Euler advance
    XB_sol = XB_A.colPivHouseholderQr().solve(XB_rhs);
    for (i_XB = 0; i_XB < 4; ++i_XB)
      state_XB[i_RU][i_XB] = XB_sol(i_XB);
  }
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <stdlib.h>
#include <math.h>
#include <typeinfo>
#include <vector>
#include <ostream>
#include <iomanip>
// to the time conversions (do not forget the files!!):
#include "time_conversion.c"
#include "time_conversion.h"

/***

    THIS IS THE DEPRECATED VERSION, PLEASE, USE THE FUNCTIONS AND STRUCTS OF THE FILE
                                "logreader2.hpp"

***/

//#include <boost/filesystem.hpp>

///
const int toUTC = 3;
///

using namespace std;

//const long double pi = 4*atan(1);
//const int gpsLibLeap = 14;

struct gpsTime
{
	unsigned short week;
	double seconds;
};

//alternative for the standart definition
struct gpsTime2
{
	int week;
	double seconds;

};

//struct that contains the INS/GNSS observations
struct obs
{

	gpsTime obsTime;

	//position
	double lat;
	double lgt;
	double h;

	//velocity
	double velX;
	double velY;
	double velZ;

	//attitude
	double pitch;    //along Y axis ("phi")
	double roll;     //along X axis ("omega")
	double azimuth;  //along Z axis ("kappa")

	//status
	string status;
};

//struct that contains the covariance matrices of the INS/GNSS observations
struct obsCov
{
	gpsTime covTime;

	double pos_mvc[3][3];
	double vel_mvc[3][3];
	double att_mvc[3][3];

};

//struct that contains all the others of the INS nested
struct obsWcov
{
	obs observation;

	obsCov obsCovariances;
};

//struct for the phototimestamp (actually of the stereopair)

struct moment
{
double hour;
double minute;
double second;
};

struct stereopairTimestamp
{
double tsyear;
double tsmonth;
double tsday;

double leapSeconds;

moment leftStart;
moment leftEnd;
moment RightStart;
moment RightEnd;

gpsTime _leftStart;
gpsTime _leftEnd;
gpsTime _RightStart;
gpsTime _RightEnd;

gpsTime finalTime;
};

//struct for the closest observations to the timestamps of the photos
struct intervals
{
double shortestNeg; int shortestNegPos;
double shortestPos; int shortestPosPos;
double theTime;
};

struct survData
{    //store the INS/GNSS data in:
	std::vector<obsWcov> pvaWcov;
	//the iterator:
	//std::vector<obsWcov>::iterator  pvaWcovIt;

	//store the stereopair timestamps
	std::vector<stereopairTimestamp> timestamps;
	//std::vector<stereopairTimestamp>::iterator timestampsIt;

	//store the shortests intervals of time
	std::vector<intervals> finalIntervals;

	    //store the final ones:
	std::vector<obsWcov> finalObsWcovs;

	string log;

	string log2_;
	double year;
	double month;
	double day;
	double LeapSeconds;

	survData(string NavLog,string PhotoTimesLog,double Year,double Month,double Day,double LeapSeconds_);

	void logReader();

	void photoTimestampReader();

	void dataInterpolation();

	};

	survData::survData(string NavLog,string PhotoTimesLog,double Year,double Month,double Day,double LeapSeconds_)
	{
        log = NavLog;
        log2_ = PhotoTimesLog;
        year = Year;
        month = Month;
        day = Day;
        LeapSeconds = LeapSeconds_;

        logReader();
        photoTimestampReader();
        dataInterpolation();

	}

//
//    //store the INS/GNSS data in:
//	std::vector<obsWcov> pvaWcov;
//	//the iterator:
//	//std::vector<obsWcov>::iterator  pvaWcovIt;
//
//
//	//store the stereopair timestamps
//	std::vector<stereopairTimestamp> timestamps;
//	//std::vector<stereopairTimestamp>::iterator timestampsIt;
//
//	//store the shortests intervals of time
//	std::vector<intervals> finalIntervals;
//
//	    //store the final ones:
//	std::vector<obsWcov> finalObsWcovs;

double linTerp(double time,double timeBefore,double timeAfter,double valBefore,double valAfter)
{
//the classic linear interpolation
double value = valBefore + (  (valAfter-valBefore)* ((time-timeBefore)/(timeAfter-timeBefore)) );

return value;
}

void survData::logReader()
{
    //3 steps, to pull of the separators
	std::ifstream infile2(log);
	string line2;
    ofstream arquivo;
	arquivo.open("parte1.txt");
	while (std::getline(infile2, line2,','))
	{
    arquivo<<line2<<endl;
	}
    infile2.close();
    arquivo.close();

    ifstream parte2;
    parte2.open("parte1.txt");
    ofstream parte2b("parte2.txt");
    string line3;
    //parte2b.open("parte2.txt");
    while (std::getline(parte2,line3,';'))
	{parte2b << line3<<endl;}
    parte2b.close(); parte2.close();
    //a.close();

    ifstream parte3;
    parte3.open("parte2.txt");
    ofstream parte3b;
    string line4;
    parte3b.open("parte3.txt");
    while (std::getline(parte3,line4,'*'))
	{parte3b << line4<<endl;}
	parte3.close();
	parte3b.close();

	//now, its possible to do the data split simpler than before
	ifstream data; data.open("parte3.txt");
	string value;
	bool itsObs=false,itsCovs=false;
	bool almostOne = false; //no observation without covariance
	int cObs=0,cCovs=0;
	obs obTemp;
	obsCov covTemp;
	obsWcov bothTemp;

	ofstream report;report.open("report.txt");

        while (std::getline(data,value))
        {
            //for the observations:
            if (value=="%INSPVASA" && almostOne)
            { //loks for an observation
                itsObs = true;
                cObs++;
                continue;
            }
            if(cObs==1 && itsObs)
            {
                obTemp.obsTime.week=stod(value);
                //obTemp.obsTime.week=stringToDouble(value);
                // //cout<<"semana: "<<stod(value)<<"   "<<value<<"   "<<endl;
                cObs++;continue;
            }
            if(cObs==2 && itsObs)
            {
                cObs++;continue;
            }
            if(cObs==3 && itsObs)
            {
                cObs++;continue;
            }
            if(cObs==4 && itsObs)
            {
                obTemp.obsTime.seconds=stod(value);
                //cout<<"segundos: "<<stringTdouble2(value)<<"   "<<value<<"   "<<endl;
                //report<<value.find('.');
                //report<<stringTdouble2(value);
                //report << StringToNumber<double> ( value ) <<endl;
                cObs++;continue;
            }
            if(cObs==5 && itsObs)
            {
                obTemp.lat=stod(value);
                //cout<<"latitude: "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==6 && itsObs)
            {
                obTemp.lgt=stod(value);
                //cout<<"longitude: "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==7 && itsObs)
            {
                obTemp.h=stod(value);
               // cout<<"h: "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==8 && itsObs)
            {
                obTemp.velX=stod(value);
                //cout<<"velX: "<<value<<endl;
                cObs++;continue;
            }
                        if(cObs==9 && itsObs)
            {
                obTemp.velY=stod(value);
                //cout<<"velY: "<<value<<endl;
                cObs++;continue;
            }
                        if(cObs==10 && itsObs)
            {
                obTemp.velZ=stod(value);
                //cout<<"velZ: "<<value<<endl;
                cObs++;continue;
            }
                    if(cObs==11 && itsObs)
            {
                obTemp.roll=stod(value);
                //cout<<"roll: "<<value<<endl;
                cObs++;continue;
            }
                if(cObs==12 && itsObs)
            {
                obTemp.pitch=stod(value);
                //cout<<"pitch: "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==13 && itsObs)
            {
                obTemp.azimuth=stod(value);
                //cout<<"azimuth: "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==14 && itsObs)
            {
                obTemp.status=value;
                //cout<<"status "<<value<<endl;
                cObs++;continue;
            }
            if(cObs==15 && itsObs)
            {
           // cout<<endl<<endl<<"FIM DE OBSERVAÇÃO"<<endl;
            bothTemp.observation=obTemp;
            bothTemp.obsCovariances=covTemp;
            pvaWcov.push_back(bothTemp);
            report <<std::setprecision(15)<< bothTemp.observation.obsTime.seconds<<"  "<<bothTemp.obsCovariances.covTime.seconds<<endl;
            //pva.push_back(obTemp);
            itsObs=false;cObs=0;continue;
            }

            //for the covariances
            if (value=="%INSCOVSA")
            { //loks for a covariance
                if (!almostOne) {almostOne=true;}
                itsCovs = true;
                cCovs++;
                continue;
            }

            if(cCovs==1 && itsCovs)
            {
                covTemp.covTime.week=stod(value);
                //covTemp.covTime.week=stringToDouble(value);
           ////     cout<<"semana: "<<stod(value)<<"   "<<value<<endl;
                cCovs++;continue;
            }
            if(cCovs==2 && itsCovs)
            {
            ////    cout<<"segundos: "<<stod(value)<<"   "<<value<<endl;
                covTemp.covTime.seconds=stod(value);
                cCovs++;continue;
            }
            if(cCovs==3 && itsCovs)
            {
                cCovs++;continue;
            }
            if(cCovs==4 && itsCovs)
            {
                cCovs++;continue;
            }
            if(cCovs==5 && itsCovs)
            {
                covTemp.pos_mvc[0][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==6 && itsCovs)
            {
                covTemp.pos_mvc[0][1]=stod(value);
           ////     cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==7 && itsCovs)
            {
                covTemp.pos_mvc[0][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==8 && itsCovs)
            {
                covTemp.pos_mvc[1][0]=stod(value);
           ////     cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==9 && itsCovs)
            {
                covTemp.pos_mvc[1][1]=stod(value);
           ////     cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==10 && itsCovs)
            {
                covTemp.pos_mvc[1][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                if(cCovs==11 && itsCovs)
            {
                covTemp.pos_mvc[2][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }

                            if(cCovs==12 && itsCovs)
            {
                covTemp.pos_mvc[2][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                        if(cCovs==13 && itsCovs)
            {
                covTemp.pos_mvc[2][2]=stod(value);
             ////   cout<<value<<endl;
                cCovs++;continue;
            }

                        if(cCovs==14 && itsCovs)
            {
                covTemp.vel_mvc[0][0]=stod(value);
             ////   cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==15 && itsCovs)
            {
                covTemp.vel_mvc[0][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==16 && itsCovs)
            {
                covTemp.vel_mvc[0][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==17 && itsCovs)
            {
                covTemp.vel_mvc[1][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==18 && itsCovs)
            {
                covTemp.vel_mvc[1][1]=stod(value);
             ////   cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==19 && itsCovs)
            {
                covTemp.vel_mvc[1][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                if(cCovs==20 && itsCovs)
            {
                covTemp.vel_mvc[2][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }

                            if(cCovs==21 && itsCovs)
            {
                covTemp.vel_mvc[2][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                        if(cCovs==22 && itsCovs)
            {
                covTemp.vel_mvc[2][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }

                        if(cCovs==23 && itsCovs)
            {
                covTemp.att_mvc[0][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==24 && itsCovs)
            {
                covTemp.att_mvc[0][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==25 && itsCovs)
            {
                covTemp.att_mvc[0][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                        if(cCovs==26 && itsCovs)
            {
                covTemp.att_mvc[1][0]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==27 && itsCovs)
            {
                covTemp.att_mvc[1][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                    if(cCovs==28 && itsCovs)
            {
                covTemp.att_mvc[1][2]=stod(value);
             ////   cout<<value<<endl;
                cCovs++;continue;
            }
                                if(cCovs==29 && itsCovs)
            {
                covTemp.att_mvc[2][0]=stod(value);
             ////   cout<<value<<endl;
                cCovs++;continue;
            }

                            if(cCovs==30 && itsCovs)
            {
                covTemp.att_mvc[2][1]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
                                        if(cCovs==31 && itsCovs)
            {
                covTemp.att_mvc[2][2]=stod(value);
            ////    cout<<value<<endl;
                cCovs++;continue;
            }
            if(cCovs==32 && itsCovs)
            {
            //cov.push_back(covTemp);
            ////cout<<endl<<endl<<"FIM DE COVARIANCIAS"<<endl;
            cCovs=0;itsCovs=false;
            }

        }

}

void survData::photoTimestampReader()
{
// if the time in UTC format are without Leap Seconds, insert "0" as the total of leap seconds
//variables for time conversion
    unsigned short  tempWeek;
    double          tempTow;
    bool            convOK;

    //the log with the final timestamps
    ofstream finalTimes("momento_da_tomada.txt");

    //first: take away the separators
    //first: ":"
    ifstream rawlog(log2_);
    string line1;
    ofstream part1("p1.txt");
        while(getline(rawlog,line1,':'))
        {
        part1<<line1<<endl;
        }
        rawlog.close();part1.close();

        //first: ";"
        ifstream log2("p1.txt");
        string line2;
        ofstream part2("p2.txt");
        while(getline(log2,line2,';'))
        {
        //if(line2 == "Start" || line2 == "End"){part2<<line2;}
        //else {part2<<line2<<endl;}
        //if(line2 == "") cout<<"existe"<<endl;
        part2<<line2<<endl;
        }
        log2.close();part2.close();

    //now, we could read the one-value-per-line file
    ifstream log3("p2.txt");
    string line3;
    moment tempLS,tempLE,tempRS,tempRE;
    stereopairTimestamp tempStamp;
    double tHour,tMin,tSec;

    //control variables
    int count=1;
    bool OneLS=false;
    bool OneLE=false;
    bool OneRS=false;
    bool OneRE=false;
    //bool isLeft=false;
    //bool isRight=false;
    bool isLeft,isRight;
    int gravou1=0;

while(getline(log3,line3))
{
//if (line3 == "")cout<<line3<<endl;

        if (count==1)
        {
                if(line3 != "")
                {tHour=stod(line3);}
        count++;continue;
        }
        if (count==2)
        {
                if(line3 != "")
                {tMin=stod(line3);}
        count++;continue;
        }
        if (count==3)
        {
                if(line3 != "")
                {tSec=stod(line3);}
        count++;continue;
        }
        if (count==4)
        {
            if(line3=="1")
            {isLeft=true;isRight=false;}
            if(line3=="2")
            {isLeft=false;isRight=true;}
        count++;continue;
        }
        if (count==5)
        {
            if(line3=="Start")
            {
                if(isLeft)
                {
                    tempLS.hour=tHour;
                    tempLS.minute=tMin;
                    tempLS.second=tSec;
//                    cout<<"passo OK"<<endl;
                    if(!OneLS){OneLS=true;}
                }
                if(isRight)
                {
                    tempRS.hour=tHour;
                    tempRS.minute=tMin;
                    tempRS.second=tSec;
                    if(!OneRS){OneRS=true;}
                }
            }
            if(line3=="End")
            {
                if(isLeft && OneLS)
                {
                    tempLE.hour=tHour;
                    tempLE.minute=tMin;
                    tempLE.second=tSec;
                    if(!OneLE){OneLE=true;}
                }
                if(isRight && OneLS)
                {
                    tempRE.hour=tHour;
                    tempRE.minute=tMin;
                    tempRE.second=tSec;
                    if(!OneRE){OneRE=true;}
                }
            }
        count++;continue;
        }
        if (count==6)
        {
                if(OneLE && OneLS && OneRE && OneRS)
                {
                //record the timestamps in UTC
                tempStamp.leftStart     = tempLS;
                tempStamp.leftEnd       = tempLE;
                tempStamp.RightStart    = tempRS;
                tempStamp.RightEnd      = tempRE;
                tempStamp.tsyear        = year;
                tempStamp.tsmonth       = month;
                tempStamp.tsday         = day;
                tempStamp.leapSeconds=LeapSeconds;
                //record the timestamps in GPS time:
                convOK =  TIMECONV_GetGPSTimeFromRinexTime(year,month,day,
                tempLS.hour+float(toUTC),tempLS.minute,tempLS.second,&tempWeek,&tempTow);
                tempStamp._leftStart.week           =   tempWeek;
                tempStamp._leftStart.seconds        =   tempTow+LeapSeconds;

                convOK =  TIMECONV_GetGPSTimeFromRinexTime(year,month,day,
                tempLE.hour+float(toUTC),tempLE.minute,tempLE.second,&tempWeek,&tempTow);
                tempStamp._leftEnd.week              =  tempWeek;
                tempStamp._leftEnd.seconds           =   tempTow+LeapSeconds;

                convOK =  TIMECONV_GetGPSTimeFromRinexTime(year,month,day,
                tempRS.hour+float(toUTC),tempRS.minute,tempRS.second,&tempWeek,&tempTow);
                tempStamp._RightStart.week           = tempWeek;
                tempStamp._RightStart.seconds        = tempTow+LeapSeconds;

                convOK =  TIMECONV_GetGPSTimeFromRinexTime(year,month,day,
                tempRE.hour+float(toUTC),tempRE.minute,tempRE.second,&tempWeek,&tempTow);
                tempStamp._RightEnd.week            = tempWeek;
                tempStamp._RightEnd.seconds         = tempTow+LeapSeconds;

                //the final timestamp:
                tempStamp.finalTime.week            =tempWeek;
                tempStamp.finalTime.seconds         =( ((tempStamp._leftStart.seconds+tempStamp._leftEnd.seconds)/2) +  ((tempStamp._RightStart.seconds+tempStamp._RightEnd.seconds)/2) ) / 2;

                timestamps.push_back(tempStamp);
                OneLS=false;OneLE=false;OneRS=false;OneRE=false;
                gravou1++;
//                cout<<gravou1<<endl;
                }
        count=1;continue;
        }

} //end of the main process of the function

finalTimes << "semana GPS: " << tempWeek<<endl<<endl;

//cout<<timestamps.size()<<endl<<endl;
for (unsigned int i=0;i < timestamps.size();i++)
{
//cout<<endl<<i<<endl<<endl;
//cout<<timestamps.at(i).leftStart.hour<<endl;
//cout<<timestamps.at(i).leftStart.minute<<endl;
//cout<<timestamps.at(i).leftStart.second<<endl;
//
//cout<<timestamps.at(i).RightStart.hour<<endl;
//cout<<timestamps.at(i).RightStart.minute<<endl;
//cout<<timestamps.at(i).RightStart.second<<endl;
//
//cout<<timestamps.at(i).leftEnd.hour<<endl;
//cout<<timestamps.at(i).leftEnd.minute<<endl;
//cout<<timestamps.at(i).leftEnd.second<<endl;
//
//cout<<timestamps.at(i).RightEnd.hour<<endl;
//cout<<timestamps.at(i).RightEnd.minute<<endl;
//cout<<timestamps.at(i).RightEnd.second<<endl;
//
//cout<<timestamps.at(i).finalTime.week<<endl;
//cout<<std::setprecision(20)<<timestamps.at(i).finalTime.seconds<<endl<<endl;
//
//cout<<std::setprecision(20)<<timestamps.at(i)._leftStart.seconds<<endl;
//
//cout<<std::setprecision(20)<<timestamps.at(i)._leftEnd.seconds<<endl;
//
//cout<<std::setprecision(20)<<timestamps.at(i)._RightStart.seconds<<endl;
//
//cout<<std::setprecision(20)<<timestamps.at(i)._RightEnd.seconds<<endl<<endl;

//cout<<day<<"    "<<month<<"     "<<year<<endl;

finalTimes<<std::setprecision(13)<<timestamps.at(i).finalTime.seconds<<endl;
}

finalTimes.close();
}

// function for the interpolation of data
void survData::dataInterpolation()
{
double delta;
int stereopair = 0;
intervals tempIntervals;
//the differences in time:
ofstream deltas("diftempo.txt");
//the final positions
ofstream finalObservations("observacoes.txt");
//arbitrary values
double minNeg = -1000; int posMinNeg;
double minPos =  1000; int posMinPos;
//temporary variables
obsWcov tempObs;
obs tempObsBef,tempObsAf;
double tBef,tAft,time1;

        for (unsigned int i=0;i < timestamps.size();i++)
        {
        stereopair++;
        deltas<<"Estereopar  "<<stereopair<<endl<<endl;
            for(unsigned  int j=0;j < pvaWcov.size();j++)
            {
            delta =pvaWcov.at(j).observation.obsTime.seconds - timestamps.at(i).finalTime.seconds;

            if (delta <= 0 && delta > minNeg)
            {
            minNeg = delta;posMinNeg=j;
            }

            if (delta >= 0 && delta < minPos)
            {
            minPos = delta;posMinPos=j;
            }

            deltas<<std::setprecision(15)<<delta<<"  "<<minNeg<<"  "<<minPos<<endl;
            }
            deltas<<endl;

            deltas<<"menor negativo:  "<<minNeg<<"  menor positivo:  "<<minPos<<" primeira: "<<posMinNeg<<" segunda "<<posMinPos<<endl;
            deltas<<endl<<endl;
            //storing that shortest intervals and these positions
            tempIntervals.shortestNeg = minNeg;tempIntervals.shortestPos = minPos;
            tempIntervals.shortestNegPos = posMinNeg; tempIntervals.shortestPosPos = posMinPos;
            tempIntervals.theTime = timestamps.at(i).finalTime.seconds;
            finalIntervals.push_back(tempIntervals);
            //reassign the temporary values:
            minNeg = -1000;minPos=1000;
        }
        deltas.close();
        //now we have the intervals of time and the positions to do the interpolation

        for(unsigned int k=0;k < finalIntervals.size();k++)
        {
        tBef = pvaWcov.at(finalIntervals.at(k).shortestNegPos).observation.obsTime.seconds;
        tAft = pvaWcov.at(finalIntervals.at(k).shortestPosPos).observation.obsTime.seconds;
        time1 = finalIntervals.at(k).theTime;

        tempObsBef  =   pvaWcov.at(finalIntervals.at(k).shortestNegPos).observation;
        tempObsAf   =   pvaWcov.at(finalIntervals.at(k).shortestPosPos).observation;

        tempObs.observation.lat     = linTerp(time1,tBef,tAft,tempObsBef.lat,tempObsAf.lat);
        tempObs.observation.lgt     = linTerp(time1,tBef,tAft,tempObsBef.lgt,tempObsAf.lgt);
        tempObs.observation.h       = linTerp(time1,tBef,tAft,tempObsBef.h,tempObsAf.h);
        tempObs.observation.roll   = linTerp(time1,tBef,tAft,tempObsBef.roll,tempObsAf.roll);
        tempObs.observation.pitch     = linTerp(time1,tBef,tAft,tempObsBef.pitch,tempObsAf.pitch);
        tempObs.observation.azimuth   = linTerp(time1,tBef,tAft,tempObsBef.azimuth,tempObsAf.azimuth);
        tempObs.observation.velX    = linTerp(time1,tBef,tAft,tempObsBef.velX,tempObsAf.velX);
        tempObs.observation.velY    = linTerp(time1,tBef,tAft,tempObsBef.velY,tempObsAf.velY);
        tempObs.observation.velZ    = linTerp(time1,tBef,tAft,tempObsBef.velZ,tempObsAf.velZ);

        finalObservations.precision(40);
        finalObservations<<tempObs.observation.lat<<",";
        finalObservations<<tempObs.observation.lgt<<",";
        finalObservations<<tempObs.observation.h<<",";
        finalObservations<<tempObs.observation.roll<<",";
        finalObservations<<tempObs.observation.pitch<<",";
        finalObservations<<tempObs.observation.azimuth<<",";
        finalObservations<<tempObs.observation.velX<<",";
        finalObservations<<tempObs.observation.velY<<",";
        finalObservations<<tempObs.observation.velZ<<endl;

        //for the covariances, the NearestNeighbour is good enough
        if (fabs(finalIntervals.at(k).shortestNeg) < fabs(finalIntervals.at(k).shortestPos)){
        tempObs.obsCovariances = pvaWcov.at(finalIntervals.at(k).shortestNegPos).obsCovariances;}
        else {tempObs.obsCovariances = pvaWcov.at(finalIntervals.at(k).shortestPosPos).obsCovariances;}

        finalObsWcovs.push_back(tempObs);
        }
//cout<<finalObsWcovs.size()<<endl;

finalObservations.close();
}