// NOLINTNEXTLINE(build/include): Its header file is included in symbolic.h.
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <ios>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>

#include <Eigen/Core>
#include <fmt/format.h>
#include <fmt/ostream.h>

#include "drake/common/drake_assert.h"
#include "drake/common/never_destroyed.h"
#include "drake/common/symbolic.h"
#define DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER
#include "drake/common/symbolic_expression_cell.h"
#undef DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER

namespace drake {
namespace symbolic {

using std::logic_error;
using std::make_shared;
using std::map;
using std::numeric_limits;
using std::ostream;
using std::ostringstream;
using std::pair;
using std::runtime_error;
using std::shared_ptr;
using std::streamsize;
using std::string;
using std::vector;

bool operator<(ExpressionKind k1, ExpressionKind k2) {
  return static_cast<int>(k1) < static_cast<int>(k2);
}

namespace {
// Negates an addition expression.
// - (E_1 + ... + E_n) => (-E_1 + ... + -E_n)
Expression NegateAddition(const Expression& e) {
  DRAKE_ASSERT(is_addition(e));
  return ExpressionAddFactory{to_addition(e)}.Negate().GetExpression();
}

// Negates a multiplication expression.
// - (c0 * E_1 * ... * E_n) => (-c0 * E_1 * ... * E_n)
Expression NegateMultiplication(const Expression& e) {
  DRAKE_ASSERT(is_multiplication(e));
  return ExpressionMulFactory{to_multiplication(e)}.Negate().GetExpression();
}
}  // namespace

shared_ptr<ExpressionCell> Expression::make_cell(const double d) {
  if (d == 0.0) {
    // The objects created by `Expression(0.0)` share the unique
    // `ExpressionConstant` object created in `Expression::Zero()`.
    //
    // See https://github.com/RobotLocomotion/drake/issues/12453 for details.
    return Expression::Zero().ptr_;
  }
  if (std::isnan(d)) {
    return make_shared<ExpressionNaN>();
  }
  return make_shared<ExpressionConstant>(d);
}

Expression::Expression(const Variable& var)
    : ptr_{make_shared<ExpressionVar>(var)} {}
Expression::Expression(const double d) : ptr_{make_cell(d)} {}
Expression::Expression(std::shared_ptr<ExpressionCell> ptr)
    : ptr_{std::move(ptr)} {}

ExpressionKind Expression::get_kind() const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->get_kind();
}

void Expression::HashAppend(DelegatingHasher* hasher) const {
  using drake::hash_append;
  hash_append(*hasher, get_kind());
  ptr_->HashAppendDetail(hasher);
}

Expression Expression::Zero() {
  static const never_destroyed<Expression> zero{
      Expression{make_shared<ExpressionConstant>(0.0)}};
  return zero.access();
}

Expression Expression::One() {
  static const never_destroyed<Expression> one{
      Expression{make_shared<ExpressionConstant>(1.0)}};
  return one.access();
}

Expression Expression::Pi() {
  static const never_destroyed<Expression> pi{
      Expression{make_shared<ExpressionConstant>(M_PI)}};
  return pi.access();
}

Expression Expression::E() {
  static const never_destroyed<Expression> e{
      Expression{make_shared<ExpressionConstant>(M_E)}};
  return e.access();
}

Expression Expression::NaN() {
  static const never_destroyed<Expression> nan{
      Expression{make_shared<ExpressionNaN>()}};
  return nan.access();
}

Variables Expression::GetVariables() const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->GetVariables();
}

bool Expression::EqualTo(const Expression& e) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  DRAKE_ASSERT(e.ptr_ != nullptr);
  if (ptr_ == e.ptr_) {
    return true;
  }
  if (get_kind() != e.get_kind()) {
    return false;
  }
  // Check structural equality.
  return ptr_->EqualTo(*(e.ptr_));
}

bool Expression::Less(const Expression& e) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  DRAKE_ASSERT(e.ptr_ != nullptr);
  if (ptr_ == e.ptr_) {
    return false;  // this equals to e, not less-than.
  }
  const ExpressionKind k1{get_kind()};
  const ExpressionKind k2{e.get_kind()};
  if (k1 < k2) {
    return true;
  }
  if (k2 < k1) {
    return false;
  }
  // k1 == k2
  return ptr_->Less(*(e.ptr_));
}

bool Expression::is_polynomial() const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->is_polynomial();
}

bool Expression::is_expanded() const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->is_expanded();
}

Expression& Expression::set_expanded() {
  DRAKE_ASSERT(ptr_ != nullptr);
  ptr_->set_expanded();
  return *this;
}

double Expression::Evaluate(const Environment& env,
                            RandomGenerator* const random_generator) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  if (random_generator == nullptr) {
    return ptr_->Evaluate(env);
  } else {
    return ptr_->Evaluate(
        PopulateRandomVariables(env, GetVariables(), random_generator));
  }
}

double Expression::Evaluate(RandomGenerator* const random_generator) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return Evaluate(Environment{}, random_generator);
}

Eigen::SparseMatrix<double> Evaluate(
    const Eigen::Ref<const Eigen::SparseMatrix<Expression>>& m,
    const Environment& env) {
  return m.unaryExpr([&env](const Expression& e) { return e.Evaluate(env); });
}

Expression Expression::EvaluatePartial(const Environment& env) const {
  if (env.empty()) {
    return *this;
  }
  Substitution subst;
  for (const pair<const Variable, double>& p : env) {
    subst.emplace(p.first, p.second);
  }
  return Substitute(subst);
}

Expression Expression::Expand() const {
  DRAKE_ASSERT(ptr_ != nullptr);
  if (ptr_->is_expanded()) {
    // If it is already expanded, return the current expression without calling
    // Expand() on the cell.
    return *this;
  } else {
    return ptr_->Expand();
  }
}

Expression Expression::Substitute(const Variable& var,
                                  const Expression& e) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->Substitute({{var, e}});
}

Expression Expression::Substitute(const Substitution& s) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  if (!s.empty()) {
    return ptr_->Substitute(s);
  }
  return *this;
}

Expression Expression::Differentiate(const Variable& x) const {
  DRAKE_ASSERT(ptr_ != nullptr);
  return ptr_->Differentiate(x);
}

RowVectorX<Expression> Expression::Jacobian(
    const Eigen::Ref<const VectorX<Variable>>& vars) const {
  RowVectorX<Expression> J(vars.size());
  for (VectorX<Variable>::Index i = 0; i < vars.size(); ++i) {
    J(i) = Differentiate(vars(i));
  }
  return J;
}

string Expression::to_string() const {
  ostringstream oss;
  oss << *this;
  return oss.str();
}

Expression operator+(Expression lhs, const Expression& rhs) {
  lhs += rhs;
  return lhs;
}

// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator+=(Expression& lhs, const Expression& rhs) {
  // Simplification: 0 + x => x
  if (is_zero(lhs)) {
    lhs = rhs;
    return lhs;
  }
  // Simplification: x + 0 => x
  if (is_zero(rhs)) {
    return lhs;
  }
  // Simplification: Expression(c1) + Expression(c2) => Expression(c1 + c2)
  if (is_constant(lhs) && is_constant(rhs)) {
    lhs = get_constant_value(lhs) + get_constant_value(rhs);
    return lhs;
  }
  // Simplification: flattening. To build a new expression, we use
  // ExpressionAddFactory which holds intermediate terms and does
  // simplifications internally.
  ExpressionAddFactory add_factory{};
  if (is_addition(lhs)) {
    // 1. (e_1 + ... + e_n) + rhs
    add_factory = to_addition(lhs);
    // Note: AddExpression method takes care of the special case where `rhs` is
    // of ExpressionAdd.
    add_factory.AddExpression(rhs);
  } else {
    if (is_addition(rhs)) {
      // 2. lhs + (e_1 + ... + e_n)
      add_factory = to_addition(rhs);
      add_factory.AddExpression(lhs);
    } else {
      // nothing to flatten: return lhs + rhs
      add_factory.AddExpression(lhs);
      add_factory.AddExpression(rhs);
    }
  }
  // Extract an expression from factory
  lhs = add_factory.GetExpression();
  return lhs;
}

Expression& Expression::operator++() {
  *this += Expression::One();
  return *this;
}

Expression Expression::operator++(int) {
  Expression copy(*this);
  ++*this;
  return copy;
}

Expression operator+(const Expression& e) { return e; }

Expression operator-(Expression lhs, const Expression& rhs) {
  lhs -= rhs;
  return lhs;
}

// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator-=(Expression& lhs, const Expression& rhs) {
  // Simplification: E - E => 0
  // TODO(soonho-tri): This simplification is not sound since it cancels `E`
  // which might cause 0/0 during evaluation.
  if (lhs.EqualTo(rhs)) {
    lhs = Expression::Zero();
    return lhs;
  }
  // Simplification: x - 0 => x
  if (is_zero(rhs)) {
    return lhs;
  }
  // Simplification: Expression(c1) - Expression(c2) => Expression(c1 - c2)
  if (is_constant(lhs) && is_constant(rhs)) {
    lhs = get_constant_value(lhs) - get_constant_value(rhs);
    return lhs;
  }
  // x - y => x + (-y)
  lhs += -rhs;
  return lhs;
}

Expression operator-(const Expression& e) {
  // Simplification: constant folding
  if (is_constant(e)) {
    return Expression{-get_constant_value(e)};
  }
  // Simplification: push '-' inside over '+'.
  // -(E_1 + ... + E_n) => (-E_1 + ... + -E_n)
  if (is_addition(e)) {
    return NegateAddition(e);
  }
  // Simplification: push '-' inside over '*'.
  // -(c0 * E_1 * ... * E_n) => (-c0 * E_1 * ... * E_n)
  if (is_multiplication(e)) {
    return NegateMultiplication(e);
  }
  return -1 * e;
}

Expression& Expression::operator--() {
  *this -= Expression::One();
  return *this;
}

Expression Expression::operator--(int) {
  // Declare as non-const to allow move.
  Expression copy(*this);
  --*this;
  return copy;
}

Expression operator*(Expression lhs, const Expression& rhs) {
  lhs *= rhs;
  return lhs;
}

// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator*=(Expression& lhs, const Expression& rhs) {
  // Simplification: 1 * x => x
  if (is_one(lhs)) {
    lhs = rhs;
    return lhs;
  }
  // Simplification: x * 1 => x
  if (is_one(rhs)) {
    return lhs;
  }
  // Simplification: (E1 / E2) * (E3 / E4) => (E1 * E3) / (E2 * E4)
  if (is_division(lhs) && is_division(rhs)) {
    lhs = (get_first_argument(lhs) * get_first_argument(rhs)) /
          (get_second_argument(lhs) * get_second_argument(rhs));
    return lhs;
  }
  // Simplification: lhs * (c / E) => (c * lhs) / E
  if (is_division(rhs) && is_constant(get_first_argument(rhs))) {
    lhs = (get_first_argument(rhs) * lhs) / get_second_argument(rhs);
    return lhs;
  }
  // Simplification: (c / E) * rhs => (c * rhs) / E
  if (is_division(lhs) && is_constant(get_first_argument(lhs))) {
    lhs = (get_first_argument(lhs) * rhs) / get_second_argument(lhs);
    return lhs;
  }
  if (is_neg_one(lhs)) {
    if (is_addition(rhs)) {
      // Simplification: push '-' inside over '+'.
      // -1 * (E_1 + ... + E_n) => (-E_1 + ... + -E_n)
      lhs = NegateAddition(rhs);
      return lhs;
    }
    if (is_multiplication(rhs)) {
      // Simplification: push '-' inside over '*'.
      // -1 * (c0 * E_1 * ... * E_n) => (-c0 * E_1 * ... * E_n)
      lhs = NegateMultiplication(rhs);
      return lhs;
    }
  }

  if (is_neg_one(rhs)) {
    if (is_addition(lhs)) {
      // Simplification: push '-' inside over '+'.
      // (E_1 + ... + E_n) * -1 => (-E_1 + ... + -E_n)
      lhs = NegateAddition(lhs);
      return lhs;
    }
    if (is_multiplication(lhs)) {
      // Simplification: push '-' inside over '*'.
      // (c0 * E_1 * ... * E_n) * -1 => (-c0 * E_1 * ... * E_n)
      lhs = NegateMultiplication(lhs);
      return lhs;
    }
  }

  // Simplification: 0 * E => 0
  // TODO(soonho-tri): This simplification is not sound since it cancels `E`
  // which might cause 0/0 during evaluation.
  if (is_zero(lhs)) {
    return lhs;
  }
  // Simplification: E * 0 => 0
  // TODO(soonho-tri): This simplification is not sound since it cancels `E`
  // which might cause 0/0 during evaluation.
  if (is_zero(rhs)) {
    lhs = Expression::Zero();
    return lhs;
  }
  // Pow-related simplifications.
  if (is_pow(lhs)) {
    const Expression& e1{get_first_argument(lhs)};
    if (is_pow(rhs)) {
      const Expression& e3{get_first_argument(rhs)};
      if (e1.EqualTo(e3)) {
        // Simplification: pow(e1, e2) * pow(e1, e4) => pow(e1, e2 + e4)
        // TODO(soonho-tri): This simplification is not sound. For example, x^4
        // * x^(-3) => x. The original expression `x^4 * x^(-3)` is evaluated to
        // `nan` when x = 0 while the simplified expression `x` is evaluated to
        // 0.
        const Expression& e2{get_second_argument(lhs)};
        const Expression& e4{get_second_argument(rhs)};
        lhs = pow(e1, e2 + e4);
        return lhs;
      }
    }
    if (e1.EqualTo(rhs)) {
      // Simplification: pow(e1, e2) * e1 => pow(e1, e2 + 1)
      // TODO(soonho-tri): This simplification is not sound.
      const Expression& e2{get_second_argument(lhs)};
      lhs = pow(e1, e2 + 1);
      return lhs;
    }
  } else {
    if (is_pow(rhs)) {
      const Expression& e1{get_first_argument(rhs)};
      if (e1.EqualTo(lhs)) {
        // Simplification: (lhs * rhs == e1 * pow(e1, e2)) => pow(e1, 1 + e2)
        // TODO(soonho-tri): This simplification is not sound.
        const Expression& e2{get_second_argument(rhs)};
        lhs = pow(e1, 1 + e2);
        return lhs;
      }
    }
  }
  if (is_constant(lhs) && is_constant(rhs)) {
    // Simplification: Expression(c1) * Expression(c2) => Expression(c1 * c2)
    lhs = Expression{get_constant_value(lhs) * get_constant_value(rhs)};
    return lhs;
  }
  // Simplification: flattening
  ExpressionMulFactory mul_factory{};
  if (is_multiplication(lhs)) {
    // (e_1 * ... * e_n) * rhs
    mul_factory = to_multiplication(lhs);
    // Note: AddExpression method takes care of the special case where `rhs` is
    // of ExpressionMul.
    mul_factory.AddExpression(rhs);
  } else {
    if (is_multiplication(rhs)) {
      // e_1 * (e_2 * ... * e_n) -> (e_2 * ... * e_n * e_1)
      //
      // Note that we do not preserve the original ordering because * is
      // associative.
      mul_factory = to_multiplication(rhs);
      mul_factory.AddExpression(lhs);
    } else {
      // Simplification: x * x => x^2 (=pow(x,2))
      if (lhs.EqualTo(rhs)) {
        lhs = pow(lhs, 2.0);
        return lhs;
      }
      // nothing to flatten
      mul_factory.AddExpression(lhs);
      mul_factory.AddExpression(rhs);
    }
  }
  lhs = mul_factory.GetExpression();
  return lhs;
}

Expression operator/(Expression lhs, const Expression& rhs) {
  lhs /= rhs;
  return lhs;
}

// NOLINTNEXTLINE(runtime/references) per C++ standard signature.
Expression& operator/=(Expression& lhs, const Expression& rhs) {
  // Simplification: x / 1 => x
  if (is_one(rhs)) {
    return lhs;
  }
  // Simplification: Expression(c1) / Expression(c2) => Expression(c1 / c2)
  if (is_constant(lhs) && is_constant(rhs)) {
    const double v1{get_constant_value(lhs)};
    const double v2{get_constant_value(rhs)};
    if (v2 == 0.0) {
      ostringstream oss{};
      oss << "Division by zero: " << v1 << "/" << v2;
      throw runtime_error(oss.str());
    }
    lhs = Expression{v1 / v2};
    return lhs;
  }
  // Simplification: E / E => 1
  // TODO(soonho-tri): This simplification is not sound since it cancels `E`
  // which might contain 0/0 problems.
  if (lhs.EqualTo(rhs)) {
    lhs = Expression::One();
    return lhs;
  }
  lhs.ptr_ = make_shared<ExpressionDiv>(lhs, rhs);
  return lhs;
}

namespace {
// Changes the precision of `os` to be the `new_precision` and saves the
// original precision so that it can be reverted when an instance of this class
// is destructed. It is used in `operator<<` of symbolic expression.
class PrecisionGuard {
 public:
  PrecisionGuard(ostream* const os, const streamsize& new_precision)
      : os_{os}, original_precision_{os->precision()} {
    os_->precision(new_precision);
  }
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PrecisionGuard)
  ~PrecisionGuard() { os_->precision(original_precision_); }

 private:
  ostream* const os_;
  const streamsize original_precision_;
};
}  // namespace

ostream& operator<<(ostream& os, const Expression& e) {
  DRAKE_ASSERT(e.ptr_ != nullptr);
  const PrecisionGuard precision_guard{&os,
                                       numeric_limits<double>::max_digits10};
  return e.ptr_->Display(os);
}

Expression log(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    const double v{get_constant_value(e)};
    ExpressionLog::check_domain(v);
    return Expression{std::log(v)};
  }
  return Expression{make_shared<ExpressionLog>(e)};
}

Expression abs(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::fabs(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionAbs>(e)};
}

Expression exp(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::exp(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionExp>(e)};
}

Expression sqrt(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    const double v{get_constant_value(e)};
    ExpressionSqrt::check_domain(v);
    return Expression{std::sqrt(v)};
  }
  // Simplification: sqrt(pow(x, 2)) => abs(x)
  if (is_pow(e)) {
    if (is_two(get_second_argument(e))) {
      return abs(get_first_argument(e));
    }
  }
  return Expression{make_shared<ExpressionSqrt>(e)};
}

Expression pow(const Expression& e1, const Expression& e2) {
  // Simplification
  if (is_constant(e2)) {
    const double v2{get_constant_value(e2)};
    if (is_constant(e1)) {
      // Constant folding
      const double v1{get_constant_value(e1)};
      ExpressionPow::check_domain(v1, v2);
      return Expression{std::pow(v1, v2)};
    }
    // pow(E, 0) => 1
    // TODO(soonho-tri): This simplification is not sound since it cancels `E`
    // which might contain 0/0 problems.
    if (v2 == 0.0) {
      return Expression::One();
    }
    // pow(E, 1) => E
    if (v2 == 1.0) {
      return e1;
    }
  }
  if (is_pow(e1)) {
    // pow(base, exponent) ^ e2 => pow(base, exponent * e2)
    const Expression& base{get_first_argument(e1)};
    const Expression& exponent{get_second_argument(e1)};
    return Expression{make_shared<ExpressionPow>(base, exponent * e2)};
  }
  return Expression{make_shared<ExpressionPow>(e1, e2)};
}

Expression sin(const Expression& e) {
  // simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::sin(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionSin>(e)};
}

Expression cos(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::cos(get_constant_value(e))};
  }

  return Expression{make_shared<ExpressionCos>(e)};
}

Expression tan(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::tan(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionTan>(e)};
}

Expression asin(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    const double v{get_constant_value(e)};
    ExpressionAsin::check_domain(v);
    return Expression{std::asin(v)};
  }
  return Expression{make_shared<ExpressionAsin>(e)};
}

Expression acos(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    const double v{get_constant_value(e)};
    ExpressionAcos::check_domain(v);
    return Expression{std::acos(v)};
  }
  return Expression{make_shared<ExpressionAcos>(e)};
}

Expression atan(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::atan(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionAtan>(e)};
}

Expression atan2(const Expression& e1, const Expression& e2) {
  // Simplification: constant folding.
  if (is_constant(e1) && is_constant(e2)) {
    return Expression{
        std::atan2(get_constant_value(e1), get_constant_value(e2))};
  }
  return Expression{make_shared<ExpressionAtan2>(e1, e2)};
}

Expression sinh(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::sinh(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionSinh>(e)};
}

Expression cosh(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::cosh(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionCosh>(e)};
}

Expression tanh(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::tanh(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionTanh>(e)};
}

Expression min(const Expression& e1, const Expression& e2) {
  // simplification: min(x, x) => x
  if (e1.EqualTo(e2)) {
    return e1;
  }
  // Simplification: constant folding.
  if (is_constant(e1) && is_constant(e2)) {
    return Expression{std::min(get_constant_value(e1), get_constant_value(e2))};
  }
  return Expression{make_shared<ExpressionMin>(e1, e2)};
}

Expression max(const Expression& e1, const Expression& e2) {
  // Simplification: max(x, x) => x
  if (e1.EqualTo(e2)) {
    return e1;
  }
  // Simplification: constant folding
  if (is_constant(e1) && is_constant(e2)) {
    return Expression{std::max(get_constant_value(e1), get_constant_value(e2))};
  }
  return Expression{make_shared<ExpressionMax>(e1, e2)};
}

Expression ceil(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::ceil(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionCeiling>(e)};
}

Expression floor(const Expression& e) {
  // Simplification: constant folding.
  if (is_constant(e)) {
    return Expression{std::floor(get_constant_value(e))};
  }
  return Expression{make_shared<ExpressionFloor>(e)};
}

Expression if_then_else(const Formula& f_cond, const Expression& e_then,
                        const Expression& e_else) {
  // simplification:: if(true, e1, e2) => e1
  if (f_cond.EqualTo(Formula::True())) {
    return e_then;
  }
  // simplification:: if(false, e1, e2) => e2
  if (f_cond.EqualTo(Formula::False())) {
    return e_else;
  }
  return Expression{make_shared<ExpressionIfThenElse>(f_cond, e_then, e_else)};
}

Expression uninterpreted_function(string name, vector<Expression> arguments) {
  return Expression{make_shared<ExpressionUninterpretedFunction>(
      std::move(name), std::move(arguments))};
}

bool is_constant(const Expression& e) { return is_constant(*e.ptr_); }
bool is_constant(const Expression& e, const double v) {
  return is_constant(e) && (to_constant(e)->get_value() == v);
}
bool is_zero(const Expression& e) { return is_constant(e, 0.0); }
bool is_one(const Expression& e) { return is_constant(e, 1.0); }
bool is_neg_one(const Expression& e) { return is_constant(e, -1.0); }
bool is_two(const Expression& e) { return is_constant(e, 2.0); }
bool is_nan(const Expression& e) { return e.get_kind() == ExpressionKind::NaN; }
bool is_variable(const Expression& e) { return is_variable(*e.ptr_); }
bool is_addition(const Expression& e) { return is_addition(*e.ptr_); }
bool is_multiplication(const Expression& e) {
  return is_multiplication(*e.ptr_);
}
bool is_division(const Expression& e) { return is_division(*e.ptr_); }
bool is_log(const Expression& e) { return is_log(*e.ptr_); }
bool is_abs(const Expression& e) { return is_abs(*e.ptr_); }
bool is_exp(const Expression& e) { return is_exp(*e.ptr_); }
bool is_sqrt(const Expression& e) { return is_sqrt(*e.ptr_); }
bool is_pow(const Expression& e) { return is_pow(*e.ptr_); }
bool is_sin(const Expression& e) { return is_sin(*e.ptr_); }
bool is_cos(const Expression& e) { return is_cos(*e.ptr_); }
bool is_tan(const Expression& e) { return is_tan(*e.ptr_); }
bool is_asin(const Expression& e) { return is_asin(*e.ptr_); }
bool is_acos(const Expression& e) { return is_acos(*e.ptr_); }
bool is_atan(const Expression& e) { return is_atan(*e.ptr_); }
bool is_atan2(const Expression& e) { return is_atan2(*e.ptr_); }
bool is_sinh(const Expression& e) { return is_sinh(*e.ptr_); }
bool is_cosh(const Expression& e) { return is_cosh(*e.ptr_); }
bool is_tanh(const Expression& e) { return is_tanh(*e.ptr_); }
bool is_min(const Expression& e) { return is_min(*e.ptr_); }
bool is_max(const Expression& e) { return is_max(*e.ptr_); }
bool is_ceil(const Expression& e) { return is_ceil(*e.ptr_); }
bool is_floor(const Expression& e) { return is_floor(*e.ptr_); }
bool is_if_then_else(const Expression& e) { return is_if_then_else(*e.ptr_); }
bool is_uninterpreted_function(const Expression& e) {
  return is_uninterpreted_function(*e.ptr_);
}

double get_constant_value(const Expression& e) {
  return to_constant(e)->get_value();
}
const Variable& get_variable(const Expression& e) {
  return to_variable(e)->get_variable();
}
const Expression& get_argument(const Expression& e) {
  return to_unary(e)->get_argument();
}
const Expression& get_first_argument(const Expression& e) {
  return to_binary(e)->get_first_argument();
}
const Expression& get_second_argument(const Expression& e) {
  return to_binary(e)->get_second_argument();
}
double get_constant_in_addition(const Expression& e) {
  return to_addition(e)->get_constant();
}
const map<Expression, double>& get_expr_to_coeff_map_in_addition(
    const Expression& e) {
  return to_addition(e)->get_expr_to_coeff_map();
}
double get_constant_in_multiplication(const Expression& e) {
  return to_multiplication(e)->get_constant();
}
const map<Expression, Expression>& get_base_to_exponent_map_in_multiplication(
    const Expression& e) {
  return to_multiplication(e)->get_base_to_exponent_map();
}

const string& get_uninterpreted_function_name(const Expression& e) {
  return to_uninterpreted_function(e)->get_name();
}

const vector<Expression>& get_uninterpreted_function_arguments(
    const Expression& e) {
  return to_uninterpreted_function(e)->get_arguments();
}

const Formula& get_conditional_formula(const Expression& e) {
  return to_if_then_else(e)->get_conditional_formula();
}

const Expression& get_then_expression(const Expression& e) {
  return to_if_then_else(e)->get_then_expression();
}

const Expression& get_else_expression(const Expression& e) {
  return to_if_then_else(e)->get_else_expression();
}

Expression operator+(const Variable& var) { return Expression{var}; }
Expression operator-(const Variable& var) { return -Expression{var}; }

VectorX<Variable> GetVariableVector(
    const Eigen::Ref<const VectorX<Expression>>& evec) {
  VectorX<Variable> vec(evec.size());
  for (int i = 0; i < evec.size(); i++) {
    const Expression e_i{evec(i)};
    if (is_variable(e_i)) {
      vec(i) = get_variable(e_i);
    } else {
      throw logic_error(fmt::format("{} is not a variable.", e_i));
    }
  }
  return vec;
}

MatrixX<Expression> Jacobian(const Eigen::Ref<const VectorX<Expression>>& f,
                             const vector<Variable>& vars) {
  DRAKE_DEMAND(!vars.empty());
  const Eigen::Ref<const VectorX<Expression>>::Index n{f.size()};
  const size_t m{vars.size()};
  MatrixX<Expression> J(n, m);
  for (int i = 0; i < n; ++i) {
    for (size_t j = 0; j < m; ++j) {
      J(i, j) = f[i].Differentiate(vars[j]);
    }
  }
  return J;
}

MatrixX<Expression> Jacobian(const Eigen::Ref<const VectorX<Expression>>& f,
                             const Eigen::Ref<const VectorX<Variable>>& vars) {
  return Jacobian(f, vector<Variable>(vars.data(), vars.data() + vars.size()));
}

namespace {

// Helper class for IsAffine functions below where an instance of this class
// is passed to Eigen::MatrixBase::visit() function.
class IsAffineVisitor {
 public:
  IsAffineVisitor() = default;
  explicit IsAffineVisitor(const Variables& variables)
      : variables_{&variables} {}

  // Called for the first coefficient. Needed for Eigen::MatrixBase::visit()
  // function.
  void init(const Expression& e, const Eigen::Index i, const Eigen::Index j) {
    (*this)(e, i, j);
  }

  // Called for all other coefficients. Needed for Eigen::MatrixBase::visit()
  // function.
  void operator()(const Expression& e, const Eigen::Index, const Eigen::Index) {
    // Note that `IsNotAffine` is only called when we have not found a
    // non-affine element yet.
    found_non_affine_element_ = found_non_affine_element_ || IsNotAffine(e);
  }

  [[nodiscard]] bool result() const { return !found_non_affine_element_; }

 private:
  // Returns true if `e` is *not* affine in variables_ (if exists) or all
  // variables in `e`.
  [[nodiscard]] bool IsNotAffine(const Expression& e) const {
    if (!e.is_polynomial()) {
      return true;
    }
    const Polynomial p{(variables_ != nullptr) ? Polynomial{e, *variables_}
                                               : Polynomial{e}};
    return p.TotalDegree() > 1;
  }

  bool found_non_affine_element_{false};
  const Variables* const variables_{nullptr};
};

}  // namespace

bool IsAffine(const Eigen::Ref<const MatrixX<Expression>>& m,
              const Variables& vars) {
  if (m.size() == 0) {
    return true;
  }
  IsAffineVisitor visitor{vars};
  m.visit(visitor);
  return visitor.result();
}

bool IsAffine(const Eigen::Ref<const MatrixX<Expression>>& m) {
  if (m.size() == 0) {
    return true;
  }
  IsAffineVisitor visitor;
  m.visit(visitor);
  return visitor.result();
}

namespace {
// Helper functions for TaylorExpand.
//
// We use the multi-index notation. Please read
// https://en.wikipedia.org/wiki/Multi-index_notation for more information.

// α = (a₁, ..., aₙ) where αᵢ ∈ Z.
using MultiIndex = vector<int>;

// Generates multi-indices of order `order` whose size is `num_vars` and append
// to `vec`. It generates the indices by increasing the elements of the given
// `base`. It only changes the i-th dimension which is greater than or equal to
// `start_dim` to avoid duplicates.
void DoEnumerateMultiIndex(const int order, const int num_vars,
                           const int start_dim, const MultiIndex& base,
                           vector<MultiIndex>* const vec) {
  DRAKE_ASSERT(order > 0);
  DRAKE_ASSERT(start_dim >= 0);
  DRAKE_ASSERT(base.size() == static_cast<size_t>(num_vars));
  if (order == 0) {
    return;
  }
  if (order == 1) {
    for (int i = start_dim; i < num_vars; ++i) {
      MultiIndex alpha = base;
      ++alpha[i];
      vec->push_back(std::move(alpha));
    }
    return;
  } else {
    for (int i = start_dim; i < num_vars; ++i) {
      MultiIndex alpha = base;
      ++alpha[i];
      DoEnumerateMultiIndex(order - 1, num_vars, i, alpha, vec);
    }
  }
}

// Returns the set of multi-indices of order `order` whose size is `num-vars`.
vector<MultiIndex> EnumerateMultiIndex(const int order, const int num_vars) {
  DRAKE_ASSERT(order > 0);
  DRAKE_ASSERT(num_vars >= 1);
  vector<MultiIndex> vec;
  MultiIndex base(num_vars, 0);  // base = (0, ..., 0)
  DoEnumerateMultiIndex(order, num_vars, 0, base, &vec);
  return vec;
}

// Computes the factorial of n.
int Factorial(const int n) {
  DRAKE_ASSERT(n >= 0);
  int f = 1;
  for (int i = 2; i <= n; ++i) {
    f *= i;
  }
  return f;
}

// Given a multi index α = (α₁, ..., αₙ), returns α₁! * ... * αₙ!.
int FactorialProduct(const MultiIndex& alpha) {
  int ret = 1;
  for (const int i : alpha) {
    ret *= Factorial(i);
  }
  return ret;
}

// Computes ∂fᵅ(a) = ∂ᵅ¹...∂ᵅⁿf(a).
Expression Derivative(Expression f, const MultiIndex& alpha,
                      const Environment& a) {
  int i = 0;
  for (const pair<const Variable, double>& p : a) {
    const Variable& v = p.first;
    for (int j = 0; j < alpha[i]; ++j) {
      f = f.Differentiate(v);
    }
    ++i;
  }
  return f.EvaluatePartial(a);
}

// Given terms = [e₁, ..., eₙ] and alpha = (α₁, ..., αₙ), returns
// pow(e₁,α₁) * ... * pow(eₙ,αₙ)
Expression Exp(const vector<Expression>& terms, const MultiIndex& alpha) {
  DRAKE_ASSERT(terms.size() == alpha.size());
  ExpressionMulFactory factory;
  for (size_t i = 0; i < terms.size(); ++i) {
    factory.AddExpression(pow(terms[i], alpha[i]));
  }
  return factory.GetExpression();
}

// Computes ∑_{|α| = order} ∂fᵅ(a) / α! * (x - a)ᵅ.
void DoTaylorExpand(const Expression& f, const Environment& a,
                    const vector<Expression>& terms, const int order,
                    const int num_vars, ExpressionAddFactory* const factory) {
  DRAKE_ASSERT(order > 0);
  DRAKE_ASSERT(terms.size() == static_cast<size_t>(num_vars));
  const vector<MultiIndex> multi_indices{EnumerateMultiIndex(order, num_vars)};
  for (const MultiIndex& alpha : multi_indices) {
    factory->AddExpression(Derivative(f, alpha, a) * Exp(terms, alpha) /
                           FactorialProduct(alpha));
  }
}
}  // namespace

Expression TaylorExpand(const Expression& f, const Environment& a,
                        const int order) {
  // The implementation uses the formulation:
  //      Taylor(f, a, order) = ∑_{|α| ≤ order} ∂fᵅ(a) / α! * (x - a)ᵅ.
  DRAKE_DEMAND(order >= 1);
  ExpressionAddFactory factory;
  factory.AddExpression(f.EvaluatePartial(a));
  const int num_vars = a.size();
  if (num_vars == 0) {
    return f;
  }
  vector<Expression> terms;  // (x - a)
  for (const pair<const Variable, double>& p : a) {
    const Variable& var = p.first;
    const double v = p.second;
    terms.push_back(var - v);
  }
  for (int i = 1; i <= order; ++i) {
    DoTaylorExpand(f, a, terms, i, num_vars, &factory);
  }
  return factory.GetExpression();
}

namespace {
// Visitor used in the implementation of the GetDistinctVariables function
// defined below.
struct GetDistinctVariablesVisitor {
  // called for the first coefficient
  void init(const Expression& value, Eigen::Index, Eigen::Index) {
    variables += value.GetVariables();
  }
  // called for all other coefficients
  void operator()(const Expression& value, Eigen::Index, Eigen::Index) {
    variables += value.GetVariables();
  }
  Variables variables;
};
}  // namespace

Variables GetDistinctVariables(const Eigen::Ref<const MatrixX<Expression>>& v) {
  GetDistinctVariablesVisitor visitor;
  v.visit(visitor);
  return visitor.variables;
}

}  // namespace symbolic

double ExtractDoubleOrThrow(const symbolic::Expression& e) {
  if (is_nan(e)) {
    // If this was a literal NaN provided by the user or a dummy_value<T>, then
    // it is sound to promote it as the "extracted value" during scalar
    // conversion.  (In contrast, if an expression tree includes a NaN term,
    // then it's still desirable to throw an exception and we should NOT return
    // NaN in that case.)
    return std::numeric_limits<double>::quiet_NaN();
  }
  return e.Evaluate();
}

}  // namespace drake


/* Copyright 2018 Nils Bore (nbore@kth.se), Yiping Xie (yipingx@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <data_tools/jsf_data.h>

#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/date_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS

#include <fcntl.h>
#ifdef _MSC_VER
  #include <io.h>
#else
  #include <unistd.h>
#endif
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <bitset>
#include <data_tools/lat_long_utm.h>

# define SONAR_MESSAGE_HEADER_START 0X1601
# define SONAR_DATA_TYPE 0X0050
# define DVL_DATA_TYPE 0X0820
# define SITUATION_DATA_TYPE 0X082A

using namespace std;

namespace jsf_data{

    jsf_sss_ping::PingsT filter_frequency(const jsf_sss_ping::PingsT& pings, int desired_freq)
    {
        jsf_sss_ping::PingsT filtered_pings;

        std::copy_if(pings.begin(), pings.end(), std::back_inserter(filtered_pings), [&](const jsf_sss_ping& ping){ return ping.frequency == desired_freq; });

        return filtered_pings;
    }

cv::Mat make_waterfall_image(const jsf_sss_ping::PingsT& pings)
{
    int rows = pings.size();
    int cols = pings[0].port.pings.size() + pings[0].stbd.pings.size();
    cv::Mat swath_img = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(255, 255, 255));
    for (int i = 0; i < pings.size(); ++i) {
        for (int j = 0; j < pings[i].port.pings.size(); ++j) {
            uchar intensity = uchar(std::min(std::max(255.*.005*pings[i].port.pings[j], 0.), 255.));
            cv::Point3_<uchar>* p = swath_img.ptr<cv::Point3_<uchar> >(i, pings[0].stbd.pings.size()+j);
            p->z = intensity;
            p->y = intensity;
            p->x = intensity;
        }
        for (int j = 0; j < pings[i].stbd.pings.size(); ++j) {
            uchar intensity = uchar(std::min(std::max(255.*.005*pings[i].stbd.pings[j], 0.), 255.));
            cv::Point3_<uchar>* p = swath_img.ptr<cv::Point3_<uchar> >(i, pings[0].stbd.pings.size()-j-1);
            p->z = intensity;
            p->y = intensity;
            p->x = intensity;
        }
    }
    cv::Mat resized_swath_img;//dst image
    cv::resize(swath_img, resized_swath_img, cv::Size(512, rows));//resize image

    return resized_swath_img;
}

void show_waterfall_image(const jsf_sss_ping::PingsT& pings)
{
    cv::Mat waterfall_image = make_waterfall_image(pings);
    cv::imshow("Waterfall image", waterfall_image);
    cv::waitKey();
}

// skip data
void skip_data(ifstream& input, jsf_msg_header jsf_hdr)
{
    int nbr_bytes;
    for (int i = 0; i < jsf_hdr.following_bytes; i++) {
        input.read(reinterpret_cast<char*>(&nbr_bytes), sizeof(char));
    }
}

jsf_sss_ping_side process_side_scan_ping_side(ifstream& input,  jsf_msg_header& jsf_hdr,  jsf_sonar_data_msg_header& jsf_sonar_data_hdr)
{
    jsf_sss_ping_side ping_side;

    ping_side.time_duration = double(jsf_sonar_data_hdr.spls_num_in_pkt - 1)*double(jsf_sonar_data_hdr.spl_intvl_in_ns)*1e-9;

    //cout << "Freq: " << jsf_sonar_data_hdr.spl_freq_in_hz << endl;

    if (jsf_sonar_data_hdr.data_format==0) {
        int16_t env_data;
        for (int i = 0; i < jsf_sonar_data_hdr.spls_num_in_pkt; ++i) {
            input.read(reinterpret_cast<char*>(&env_data), sizeof(env_data));
            ping_side.pings.push_back(ldexpf((float)env_data, -jsf_sonar_data_hdr.weighting_factor_n));

        }
    }
    else if (jsf_sonar_data_hdr.data_format==1) {
        cout << "Data format: " << jsf_sonar_data_hdr.data_format << "has real and imginary part, stored in pings and pings_phase respectively" << endl;
        int16_t analytic_sig_data[2];

        for (int i = 0; i < jsf_sonar_data_hdr.spls_num_in_pkt; ++i) {
            input.read(reinterpret_cast<char*>(&analytic_sig_data), sizeof(analytic_sig_data));
            ping_side.pings.push_back(ldexpf((float)analytic_sig_data[0], -jsf_sonar_data_hdr.weighting_factor_n));
            ping_side.pings_phase.push_back(ldexpf((float)analytic_sig_data[1], -jsf_sonar_data_hdr.weighting_factor_n));
        }
    }
    else{
        cout << "Skip data format: " << jsf_sonar_data_hdr.data_format << endl;
        for (int i = 0; i < jsf_hdr.following_bytes-sizeof(jsf_sonar_data_hdr); i++) {
            int16_t nbr_bytes;
            input.read(reinterpret_cast<char*>(&nbr_bytes), sizeof(char));
        }
    }
    return ping_side;
 }

template <typename ReturnType, typename JsfHeaderType>
ReturnType read_datagram(std::ifstream& input,  JsfHeaderType& header,  jsf_msg_header& jsf_hdr)
{
    ReturnType rtn;
	return rtn;
}

template <typename ReturnType, typename JsfHeaderType, int Code>
vector<ReturnType, Eigen::aligned_allocator<ReturnType> > parse_file_impl(const boost::filesystem::path& path)
{
    vector<ReturnType, Eigen::aligned_allocator<ReturnType> > returns;
    if (boost::filesystem::extension(path) != ".JSF" && boost::filesystem::extension(path) != ".jsf") {
        cout << "Not an .JSF file, skipping..." << endl;
        cout << "Extension: " << boost::filesystem::extension(path) << endl;
        return returns;
    }

    ifstream input;
    input.open(path.string(), ios::binary);
    if (input.fail()) {
        cout << "ERROR: Cannot open the file..." << endl;
        exit(0);
        return returns;
    }
    while (!input.eof()) {
        jsf_msg_header jsf_hdr;
        input.read(reinterpret_cast<char*>(&jsf_hdr),sizeof(jsf_hdr));
        if (jsf_hdr.start_marker != SONAR_MESSAGE_HEADER_START) {
            cout << "Invalid file format! start marker: " << jsf_hdr.start_marker << endl;
            break;
        }

        if (jsf_hdr.msg_type == Code) {
            JsfHeaderType header;
            input.read(reinterpret_cast<char*>(&header), sizeof(header));
            returns.push_back(read_datagram<ReturnType, JsfHeaderType>(input, header, jsf_hdr));
            returns.back().first_in_file_ = false;
        }
        else {
            skip_data(input,jsf_hdr);
        }
    }

    if (!returns.empty()) {
        returns[0].first_in_file_ = true;
    }

	return returns;

}

template <>
jsf_sss_ping read_datagram<jsf_sss_ping, jsf_sonar_data_msg_header>(std::ifstream& input,  jsf_sonar_data_msg_header& jsf_sonar_data_hdr,  jsf_msg_header& jsf_hdr)
{
    jsf_sss_ping ping;
    jsf_sss_ping_side ping_side;
    ping.frequency = jsf_sonar_data_hdr.spl_freq_in_hz;
    ping.sound_vel = jsf_sonar_data_hdr.sound_speed_in_m_per_s;
    ping.rpy = Eigen::Vector3d(jsf_sonar_data_hdr.roll, jsf_sonar_data_hdr.pitch, jsf_sonar_data_hdr.compass_heading);
    ping.rpy.head<2>() = M_PI/32768.*ping.rpy.head<2>();
    ping.rpy[2] = .5*M_PI - M_PI/180.*0.01*ping.rpy[2];

    // NOTE: this is only valid if coord_units == 2
    ping.lat_ = 0.0001/60.*double(jsf_sonar_data_hdr.y_coord);
    ping.long_ = 0.0001/60.*double(jsf_sonar_data_hdr.x_coord);
    double easting, northing;
    string utm_zone;
    tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_);
    //cout << "UTM ZONE: " << utm_zone << endl;
    ping.utm_zone = utm_zone;
    ping.pos_ = Eigen::Vector3d(easting, northing, -0.001*jsf_sonar_data_hdr.depth_in_mm);
    ping_side = process_side_scan_ping_side(input, jsf_hdr, jsf_sonar_data_hdr);

    //cout << "Coord units: " << jsf_sonar_data_hdr.coord_units << endl;

    const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");
    boost::posix_time::ptime data_time;

    if (jsf_hdr.prot_ver >= 8) {
        data_time = epoch + boost::posix_time::seconds(jsf_sonar_data_hdr.ping_time_in_sec) + boost::posix_time::milliseconds(jsf_sonar_data_hdr.today_in_ms%1000);
    }
    else {
        boost::posix_time::ptime data_time(boost::gregorian::date(jsf_sonar_data_hdr.cpu_year, 1, 1), boost::posix_time::hours(jsf_sonar_data_hdr.cpu_day*24-24)+boost::posix_time::minutes(0)+boost::posix_time::seconds(0)+boost::posix_time::milliseconds(jsf_sonar_data_hdr.today_in_ms));
    }
    stringstream time_ss;
    time_ss << data_time;
    ping.time_string_ = time_ss.str();
    boost::posix_time::time_duration const diff = data_time - epoch;
    ping.time_stamp_ = diff.total_milliseconds();

    if (jsf_hdr.channel_num == 0) {
        ping.port = ping_side;
    }
    else {
        ping.stbd = ping_side;
    }

    ping.first_in_file_ = false;

    return ping;

}

template <>
jsf_dvl_ping read_datagram<jsf_dvl_ping, jsf_dvl_msg_header>(std::ifstream& input,  jsf_dvl_msg_header& jsf_dvl_msg,  jsf_msg_header& jsf_hdr)
{
    jsf_dvl_ping ping;
    const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");
    boost::posix_time::ptime data_time;

    data_time = epoch + boost::posix_time::seconds(jsf_dvl_msg.time_in_sec) + boost::posix_time::milliseconds(jsf_dvl_msg.ms_in_cur_sec);

    stringstream time_ss;
    time_ss << data_time;
    ping.time_string_ = time_ss.str();
    boost::posix_time::time_duration const diff = data_time - epoch;
    ping.time_stamp_ = diff.total_milliseconds();

    ping.vel_wrt_bottom_ = Eigen::Vector3d(jsf_dvl_msg.x_vel_wrt_bottom/1000., jsf_dvl_msg.y_vel_wrt_bottom/1000., jsf_dvl_msg.z_vel_wrt_bottom/1000.);
    ping.vel_wrt_water_ = Eigen::Vector3d(jsf_dvl_msg.x_vel_wrt_water/1000., jsf_dvl_msg.y_vel_wrt_water/1000., jsf_dvl_msg.z_vel_wrt_water/1000.);
    ping.dist_to_bottom_ = Eigen::Vector4d(jsf_dvl_msg.dist_to_bottom_in_cm[0]/100., jsf_dvl_msg.dist_to_bottom_in_cm[1]/100,jsf_dvl_msg.dist_to_bottom_in_cm[2]/100.,jsf_dvl_msg.dist_to_bottom_in_cm[3]/100.);
    ping.depth_ = jsf_dvl_msg.depth_in_dm/10.;
    ping.pitch_ = jsf_dvl_msg.pitch/100./180.*M_PI;
    ping.roll_ = jsf_dvl_msg.roll/100./180.*M_PI;
    ping.heading_ = jsf_dvl_msg.heading/100./180.*M_PI;
    ping.salinity_ = jsf_dvl_msg.salinity;
    ping.temp_ = jsf_dvl_msg.temp/100.;
    ping.sound_vel_ = jsf_dvl_msg.sound_vel;

    // initialize the flag
    ping.flag_["Vxy"] = false; // bit 0
    ping.flag_["Vz"] = false; // bit 2
    ping.flag_["Vxy_water"] = false; // bit 3
    ping.flag_["Vz_water"] = false; // bit 4
    ping.flag_["dist_bottom"] = false; // bit 5
    ping.flag_["heading"] = false; // bit 6
    ping.flag_["pitch"] = false; // bit 7
    ping.flag_["roll"] = false; // bit 8
    ping.flag_["temp"] = false; // bit 9
    ping.flag_["depth"] = false; // bit 10
    ping.flag_["salinity"] = false; // bit 11
    ping.flag_["sound_vel"] = false; // bit 12

    ping.ship_coord_ = false; // bit 1
    ping.error_ = false; // bit 31

    // bit 0
    if (jsf_dvl_msg.flag & 0x01) ping.flag_["Vxy"] = true;

    // bit 1
    if (jsf_dvl_msg.flag & 0x02) ping.ship_coord_ = true;

    // bit 2
    if (jsf_dvl_msg.flag & 0x04) ping.flag_["Vz"] = true;

    // bit 3
    if (jsf_dvl_msg.flag & 0x08) ping.flag_["Vxy_water"] = true;

    // bit 4
    if (jsf_dvl_msg.flag & 0x10) ping.flag_["Vz_water"] = true;

    // bit 5
    if (jsf_dvl_msg.flag & 0x20) ping.flag_["dist_bottom"] = true;

    // bit 6
    if (jsf_dvl_msg.flag & 0x40) ping.flag_["heading"] = true;

    // bit 7
    if (jsf_dvl_msg.flag & 0x80) ping.flag_["pitch"] = true;

    // bit 8
    if (jsf_dvl_msg.flag & 0x100) ping.flag_["roll"] = true;

    // bit 9
    if (jsf_dvl_msg.flag & 0x200) ping.flag_["temp"] = true;

    // bit 10
    if (jsf_dvl_msg.flag & 0x400) ping.flag_["depth"] = true;

    // bit 11
    if (jsf_dvl_msg.flag & 0x800) ping.flag_["salinity"] = true;

    // bit 12
    if (jsf_dvl_msg.flag & 0x1000) ping.flag_["sound_vel"] = true;

    // bit 31
    if(jsf_dvl_msg.flag & 0x100000000){
        cout << "Error detected! flag in decimal: " << fixed << jsf_dvl_msg.flag << endl;
        ping.error_ = true;
    }

    return ping;

}

std_data::sss_ping::PingsT convert_to_xtf_pings(const jsf_sss_ping::PingsT& pings)
{
    std_data::sss_ping::PingsT converted;
    converted.reserve(pings.size());

    for (const jsf_sss_ping& ping : pings) {
        std_data::sss_ping cping;
        cping.time_stamp_ = ping.time_stamp_;
        cping.time_string_ = ping.time_string_;
        cping.first_in_file_ = ping.first_in_file_;
        cping.roll_ = ping.rpy[0];
        cping.pitch_ = ping.rpy[1];
        cping.heading_ = ping.rpy[2];
        cping.lat_ = ping.lat_;
        cping.long_ = ping.long_;
        cping.pos_ = ping.pos_;
        cping.sound_vel_ = ping.sound_vel;

        cping.stbd.time_duration = ping.stbd.time_duration;
        cping.stbd.pings.resize(ping.stbd.pings.size());
        for (int i = 0; i < ping.stbd.pings.size(); ++i)
        {
            cping.stbd.pings[i] = int(2.*32767.*(.005*ping.stbd.pings[i] - .5));
        }

        cping.port.time_duration = ping.port.time_duration;
        cping.port.pings.resize(ping.port.pings.size());
        for (int i = 0; i < ping.port.pings.size(); ++i)
        {
            cping.port.pings[i] = int(2.*32767.*(.005*ping.port.pings[i] - .5));
        }

        converted.push_back(cping);
    }

    return converted;
}

} // namespace jsf_data

namespace std_data {

using namespace jsf_data;

template <>
jsf_sss_ping::PingsT parse_file<jsf_sss_ping>(const boost::filesystem::path& file)
{
    jsf_sss_ping::PingsT pings = parse_file_impl<jsf_sss_ping, jsf_sonar_data_msg_header, 80>(file);
    jsf_sss_ping::PingsT fixed_pings;
    int start;
    if ((pings[0].time_stamp_ - pings[1].time_stamp_) < 2) {
        start = 0;
    }
    else if ((pings[1].time_stamp_ - pings[2].time_stamp_) < 2) {
        start = 1;
    }
    for (int i = start; i < pings.size()-1; i+=2) {
        jsf_sss_ping fixed_ping;
        if (pings[i].port.pings.size() != 0 && pings[i+1].stbd.pings.size() != 0) {

            fixed_ping = pings[i];
            fixed_ping.stbd = pings[i+1].stbd;
            fixed_pings.push_back(fixed_ping);
        }
        else if (pings[i].stbd.pings.size() != 0 && pings[i+1].port.pings.size() != 0) {
            fixed_ping = pings[i];
            fixed_ping.port = pings[i+1].port;
            fixed_pings.push_back(fixed_ping);
        }
        else {
            cout << "Invalid data format! Channel numbers are not 0 and 1!" << endl;
            break;
        }
    }
    return fixed_pings;
}

template <>
jsf_dvl_ping::PingsT parse_file<jsf_dvl_ping>(const boost::filesystem::path& file)
{
    return parse_file_impl<jsf_dvl_ping, jsf_dvl_msg_header, 2080>(file);
}

} // namespace std_data


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <votca/xtp/forces.h>
#include <boost/format.hpp>

namespace votca {
    namespace xtp {

        void Forces::Initialize(Property *options) {

            // checking if there is only one segment
            _nsegments = _segments.size();
            if (_nsegments > 1) throw runtime_error(string("\n Force calculation for more than 1 conjugated segment not supported. Stopping!"));

            // pre-check forces method
            std::vector<string> choices = {"forward", "central"};
            _force_method = options->ifExistsAndinListReturnElseThrowRuntimeError<string>(".method", choices);

            // output level
            _noisy_output = options->ifExistsReturnElseReturnDefault<bool>(".noisy", false);

            // precaution in case we implement approx. analytic forces in the future
            if ((_force_method == "forward") || (_force_method == "central")) {
                _displacement = options->ifExistsReturnElseReturnDefault<double>(".displacement", 0.001); // Angstrom
            }

            // check for force removal options
            choices = {"total", "CoM", "none"};
            string _force_removal = options->ifExistsAndinListReturnElseThrowRuntimeError<string>(".removal", choices);
            if (_force_removal == "total") _remove_total_force = true;
            if (_force_removal == "CoM") _remove_CoM_force = true;

            _natoms = _segments[0]->Atoms().size();
            _forces = ub::zero_matrix<double>(_natoms, 3);

            return;

        }

        void Forces::Calculate(const double& energy) {

            ctp::TLogLevel _ReportLevel = _pLog->getReportLevel(); // backup report level
            if ( ! _noisy_output ){
                _pLog->setReportLevel(ctp::logERROR); // go silent for force calculations
            }

            //backup current coordinates (WHY?)
            std::vector <ctp::Segment* > _molecule;
            ctp::Segment _current_coordinates(0, "mol");
            _qminterface.Orbitals2Segment(&_current_coordinates, _orbitals);
            _molecule.push_back(&_current_coordinates);

            // displace all atoms in each Cartesian coordinate and get new energy
            std::vector< ctp::Atom* > _atoms;
            std::vector< ctp::Atom* > ::iterator ait;
            _atoms = _current_coordinates.Atoms();

            int _i_atom = 0;
            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {

                if ( _noisy_output ){
                    CTP_LOG(ctp::logINFO, *_pLog) << "FORCES--DEBUG working on atom " << _i_atom << flush;
                }

                // Calculate Force on this atom
                ub::matrix_range< ub::matrix<double> > _force = ub::subrange(_forces, _i_atom, _i_atom + 1, 0, 3);
                if (_force_method == "forward") NumForceForward(energy, ait, _force, _molecule);
                if (_force_method == "central") NumForceCentral(energy, ait, _force, _molecule);
                _i_atom++;
            }

            _pLog->setReportLevel(_ReportLevel); //

            // Remove Total Force, if requested
            if (_remove_total_force) RemoveTotalForce();
            //Report();

            return;
        }

        void Forces::Report() {

            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("   ---- FORCES (Hartree/Bohr)   ")).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("        %1$s differences   ") % _force_method).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("        displacement %1$1.4f Angstrom   ") % _displacement).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("   Atom\t x\t  y\t  z ")).str() << flush;

            for (unsigned _i = 0; _i < _forces.size1(); _i++) {
                CTP_LOG(ctp::logINFO, *_pLog) << (boost::format(" %1$4d    %2$+1.4f  %3$+1.4f  %4$+1.4f")
                        % _i % _forces(_i, 0) % _forces(_i, 1) % _forces(_i, 2)).str() << flush;
            }

            return;

        }

        /* Calculate forces on an atom numerically by forward differences */
        void Forces::NumForceForward(double energy, std::vector< ctp::Atom* > ::iterator ait, ub::matrix_range< ub::matrix<double> >& _force,
                std::vector<ctp::Segment*> _molecule) {

            // get this atoms's current coordinates
            vec _current_pos = (*ait)->getQMPos(); // in nm

            for (unsigned _i_cart = 0; _i_cart < 3; _i_cart++) {

                // get displacement std::vector
                vec _displaced(0, 0, 0);
                if (_i_cart == 0) {
                    _displaced.setX(_displacement * tools::conv::ang2nm); // x, _displacement in Angstrom, now in nm
                }
                if (_i_cart == 1) {
                    _displaced.setY(_displacement * tools::conv::ang2nm); // y, _displacement in in Angstrom, now in nm
                }
                if (_i_cart == 2) {
                    _displaced.setZ(_displacement * tools::conv::ang2nm); // z, _displacement in in Angstrom, now in nm
                }

                // update the coordinate
                vec _pos_displaced = _current_pos + _displaced;

                (*ait)->setQMPos(_pos_displaced); // put updated coordinate into segment

                // run DFT and GW-BSE for this geometry
                _gwbse_engine.ExcitationEnergies(_qmpackage, _molecule, _orbitals);

                // get total energy for this excited state
                double energy_displaced = _orbitals->GetTotalEnergy(_spin_type, _opt_state);

                // calculate force and put into matrix
                _force(0, _i_cart) = (energy - energy_displaced) / (_displacement * votca::tools::conv::ang2bohr); // force a.u./a.u.
                (*ait)->setQMPos(_current_pos); // restore original coordinate into segment
            } // Cartesian directions
            return;
        }

        /* Calculate forces on atoms numerically by central differences */
        void Forces::NumForceCentral(double energy, std::vector< ctp::Atom* > ::iterator ait, ub::matrix_range< ub::matrix<double> >& _force,
                std::vector<ctp::Segment*> _molecule) {

            vec _current_pos = (*ait)->getQMPos(); // in nm

            // go through all cartesian components
            for (unsigned _i_cart = 0; _i_cart < 3; _i_cart++) {

                if ( _noisy_output ){
                    CTP_LOG(ctp::logINFO, *_pLog) << "FORCES--DEBUG           Cartesian component " << _i_cart << flush;
                }

                // get displacement vector in positive direction
                vec _displaced(0, 0, 0);
                if (_i_cart == 0) {
                    _displaced.setX(_displacement * tools::conv::ang2nm); // x, _displacement in Bohr
                }
                if (_i_cart == 1) {
                    _displaced.setY(_displacement * tools::conv::ang2nm); // y, _displacement in in Angstrom
                }
                if (_i_cart == 2) {
                    _displaced.setZ(_displacement * tools::conv::ang2nm); // z, _displacement in in Angstrom
                }

                // update the coordinate
                vec _pos_displaced = _current_pos + _displaced;
                (*ait)->setQMPos(_pos_displaced); // put updated coordinate into segment

                // run DFT and GW-BSE for this geometry
                _gwbse_engine.ExcitationEnergies(_qmpackage, _molecule, _orbitals);

                // get total energy for this excited state
                double energy_displaced_plus = _orbitals->GetTotalEnergy(_spin_type, _opt_state);

                // get displacement vector in negative direction

                // update the coordinate
                _pos_displaced = _current_pos - 2.0 * _displaced;
                (*ait)->setQMPos(_pos_displaced); // put updated coordinate into segment

                // run DFT and GW-BSE for this geometry
                _gwbse_engine.ExcitationEnergies(_qmpackage, _molecule, _orbitals);

                // get total energy for this excited state
                double energy_displaced_minus = _orbitals->GetTotalEnergy(_spin_type, _opt_state);

                // calculate force and put into matrix
                _force(0, _i_cart) = 0.5 * (energy_displaced_minus - energy_displaced_plus) / (_displacement * votca::tools::conv::ang2bohr); // force a.u./a.u.

                (*ait)->setQMPos(_current_pos); // restore original coordinate into segment
            }

            return;
        }

        /* Adjust forces so that sum of forces is zero */
        void Forces::RemoveTotalForce() {

            // total force on all atoms
            ub::vector<double> _total_force = TotalForce();

            // zero total force
            for (unsigned _i_atom = 0; _i_atom < _natoms; _i_atom++) {
                for (unsigned _i_cart = 0; _i_cart < 3; _i_cart++) {
                    _forces(_i_atom, _i_cart) -= _total_force(_i_cart) / _natoms;
                }
            }
            return;
        }

        /* Determine Total Force on all atoms */
        ub::vector<double> Forces::TotalForce() {

            ub::vector<double> _total_force(3, 0.0);
            for (unsigned _i_atom = 0; _i_atom < _natoms; _i_atom++) {
                for (unsigned _i_cart = 0; _i_cart < 3; _i_cart++) {
                    _total_force(_i_cart) += _forces(_i_atom, _i_cart);
                }
            }
            return _total_force;
        }
    }
}


/*
* @file			GTA.cpp
* @main author  Sejoon Oh (ohhenrie@snu.ac.kr), Seoul National University
* @author       Namyong Park (namyongp@cs.cmu.edu), Carnegie Mellon University
* @author		Jun-gi Jang (elnino4@snu.ac.kr), Seoul National University
* @author       Lee Sael (saellee@gmail.com), Seoul National University
* @author       U Kang (ukang@snu.ac.kr), Seoul National University
* @version      1.1
* @date         2019-04-24
*
* A General Framework for Tucker Factorization on Heterogeneous Platforms
*
* This software is free of charge under research purposes.
* For commercial purposes, please contact the main author.
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <CL/cl.h>
#include "omp.h"
#include <time.h>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
typedef Matrix<double, Dynamic, Dynamic> MatrixXdd;
#define lambda 0.001

struct GPU_objects {
	cl_platform_id platform;
	cl_device_id device;
	cl_device_id *devices;
	cl_context context;
	cl_command_queue queue;
	cl_command_queue *queues;
	cl_program program;
	char *kernel_source;
	size_t kernel_source_size;
	cl_kernel kernel;
	cl_kernel *kernels;
	cl_int err;
	cl_uint num_devices;
}TF,RECON;

struct Tensor{
	int order;											// Tensor order (e.g., 5)
	int nonzeros;										// Number of nonzeros in a tensor (e.g., 1000000)
	int gpu_mode;										// CPU:0, Single-GPU: 1, Multi-GPU: 2
	int partially_observed;								// 0: fully observed, 1: partially observed (default)
	int local_size;
	int *dimension;										// Tensor dimensionality (e.g., 100x100x100)
	//FOR COO format
	int *index;											// Indices of a tensor (e.g., (1,2,3))
	float *value;										// Values of a tensor (e.g., 4.5)
	//FOR CS-N format
	float *FactorM;
}X,CoreT;

#define CHECK_ERROR(err) \
  if (err != CL_SUCCESS) { \
    printf("[%s:%d] OpenCL error %d\n", __FILE__, __LINE__, err); \
    exit(EXIT_FAILURE); \
  }

double ffrand(double x, double y) {//return the random value in (x,y) interval
	return ((y - x)*((double)rand() / RAND_MAX)) + x;
}
double abss(double x) {
	return x > 0 ? x : -x;
}

double Fit, pFit = -1;
int max_dim;
int *WhereX, *CountX;					        // WhereX[n][I_n] contains all entries of a tensor X whose nth mode's index is I_n

double copy_time, gpu_time;

int rrank,l_size,gpu_mode,nnz_mode;
char* InputPath;
char* ResultPath;

/////////////////////////////////        OpenCL-related Variables            /////////////////////////////////
cl_context context;
cl_command_queue *queues;
cl_program program;
char *kernel_source;
size_t kernel_source_size;
cl_kernel *kernels;
cl_int err;
cl_uint num_devices;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

//[Input] Source code
//[Output] Source code in a character format
//[Function] Convert source code into a string
char * get_source_code(const char *file_name, size_t *len) {
	char *source_code;
	size_t length;
	FILE *file = fopen(file_name, "rb");
	if (file == NULL) {
		printf("[%s:%d] Failed to open %s\n", __FILE__, __LINE__, file_name);
		exit(EXIT_FAILURE);
	}

	fseek(file, 0, SEEK_END);
	length = (size_t)ftell(file);
	rewind(file);

	source_code = (char *)malloc(length + 1);
	fread(source_code, length, 1, file);
	source_code[length] = '\0';

	fclose(file);

	*len = length;
	return source_code;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
//[Input] Existing contents of a tensor
//[Output] Tensor in a structured form
//[Function] Constructor function
void Construct_Core_Tensor(int ord, int nnz, int* dim, int* ind, float* val) {
	CoreT.order = ord; CoreT.nonzeros = nnz; CoreT.dimension = dim; CoreT.index = ind; CoreT.value = val;
}
//[Input] Path of a tensor and its metadata
//[Output] Tensor in a structured form
//[Function] Constructor function
void Read_Tensor(char* Path) {
	printf("Reading Input Tensor......\n");
	FILE* fin = fopen(Path, "r");
	FILE* fin2 = fopen(Path,"r");
	char tmp[10005];
	int i, j,k;
	float v;
	int pos = 0,len;
	X.nonzeros=X.order=0;
	while(fgets(tmp,10005,fin2)){
		X.nonzeros++;
		len = strlen(tmp);
		if(X.nonzeros==1){
			for(i=0;i<len;i++){
				if(tmp[i]==' ' || tmp[i]=='\t'){
					X.order++;
				}
			}
		}
	}
	X.dimension = (int *)malloc(sizeof(int)*X.order);
	X.index = (int *)malloc(sizeof(int)*X.order*X.nonzeros);
	X.value = (float *)malloc(sizeof(float)*X.nonzeros);
	X.gpu_mode = 0;
	for (i = 0; i < X.order; i++) X.dimension[i] = 0;
	for (i = 0; i < X.nonzeros; i++) {
		fgets(tmp, 10005, fin);
		len = strlen(tmp);
		int k = 0, idx = 0, flag = 0, flag2 = 0;
		double mul = 0.1, val = 0;
		for (j = 0; j < len; j++) {
			if (tmp[j] == ' ' || tmp[j] == '\t') {
				X.index[pos++] = idx - 1;
				if (X.dimension[k] < idx) X.dimension[k] = idx;
				idx = 0;
				k++;
			}
			else if (tmp[j] >= '0' && tmp[j] <= '9') {
				if (flag == 1) {
					val += mul*(tmp[j] - '0');
					mul /= 10;
				}
				else idx = idx * 10 + tmp[j] - '0';
			}
			else if (tmp[j] == '.') {
				val += idx;
				flag = 1;
			}
			else if (tmp[j] == '-') {
				flag2 = 1;
			}
		}
		if(flag==0) val = idx;
        if(flag2 == 0){
		    X.value[i] = val;
        }else{
            X.value[i] = -val;
        }
	}
	X.partially_observed = nnz_mode;
	X.local_size = l_size;
	X.gpu_mode = gpu_mode;
	printf("Reading DONE!\n\n");
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
//[Input] GPU mode and kernel information
//[Output] Initialized OpenCL variables
//[Function] Setup and initialized Opencl-related variables
GPU_objects GPU_INIT(int gpu_mode, char* kernel_code, char* kernel_name) {
	GPU_objects obj;
	err = clGetPlatformIDs(1, &obj.platform, NULL); CHECK_ERROR(err);
	err = clGetDeviceIDs(obj.platform, CL_DEVICE_TYPE_GPU, 0, NULL, &obj.num_devices); CHECK_ERROR(err);
	printf("%u devices\n", obj.num_devices);
	obj.devices = (cl_device_id*)malloc(sizeof(cl_device_id) * obj.num_devices);
	obj.queues = (cl_command_queue*)malloc(sizeof(cl_command_queue) * obj.num_devices);
	obj.kernels = (cl_kernel*)malloc(sizeof(cl_kernel) * obj.num_devices);
	err = clGetDeviceIDs(obj.platform, CL_DEVICE_TYPE_GPU, obj.num_devices, obj.devices, NULL); CHECK_ERROR(err);
	obj.context = clCreateContext(NULL, obj.num_devices, obj.devices, NULL, NULL, &err); CHECK_ERROR(err);
	for (int i = 0; i < obj.num_devices; i++) {
		obj.queues[i] = clCreateCommandQueue(obj.context, obj.devices[i], 0, &err);	CHECK_ERROR(err);
	}
	obj.kernel_source = get_source_code(kernel_code, &obj.kernel_source_size);
	obj.program = clCreateProgramWithSource(obj.context, 1, (const char**)&obj.kernel_source, &obj.kernel_source_size, &err); CHECK_ERROR(err);
	err = clBuildProgram(obj.program, obj.num_devices, obj.devices, "", NULL, NULL);
	if (err == CL_BUILD_PROGRAM_FAILURE) {
		size_t log_size;
		char *log;
		err = clGetProgramBuildInfo(obj.program, obj.devices[0], CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size); CHECK_ERROR(err);
		log = (char*)malloc(log_size + 1);
		err = clGetProgramBuildInfo(obj.program, obj.devices[0], CL_PROGRAM_BUILD_LOG, log_size, log, NULL); CHECK_ERROR(err);
		log[log_size] = '\0';
		printf("Compiler error:\n%s\n", log);
		free(log);
		exit(0);
	}
	for (int i = 0; i < obj.num_devices; i++) {
		obj.kernels[i] = clCreateKernel(obj.program, kernel_name, &err); CHECK_ERROR(err);
	}
	return obj;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
//[Input] GPU mode and OpenCL variables
//[Output] Released OpenCL variables
//[Function] Release the corresponding OpenCL variables
void GPU_DONE(int mode,GPU_objects obj) {
	if (mode == 0) return;
	int i = 0;
	for (i = 0; i < obj.num_devices; i++) clReleaseKernel(obj.kernels[i]);
	free(obj.kernels);
	for (i = 0; i < obj.num_devices; i++) clReleaseCommandQueue(obj.queues[i]);
	free(obj.queues);
	free(obj.devices);
	clReleaseProgram(obj.program);
	clReleaseContext(obj.context);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
void assign_index() {

	int order = X.order, nonzeros = X.nonzeros;
	int* indices = X.index;
	int *tempX = (int *)malloc(sizeof(int)*max_dim*order);
	int pos = 0, i, j, k, l;
	for (i = 0; i < order; i++) {
		for (j = 0; j < X.dimension[i]; j++) {
			CountX[i*max_dim + j] = tempX[i*max_dim + j] = 0;
		}
	}
	for (i = 0; i < nonzeros; i++) {
		for (j = 0; j < order; j++) {
			k = indices[pos++];
			CountX[j*max_dim + k]++;
			tempX[j*max_dim + k]++;
		}
	}
	pos = 0;
	int now = 0;
	for (i = 0; i < order; i++) {
		pos = i*max_dim;
		for (j = 0; j < X.dimension[i]; j++) {
			k = CountX[pos];
			CountX[pos] = now;
			tempX[pos++] = now;
			now += k;
		}
		CountX[pos] = now;
		tempX[pos] = now;
	}
	pos = 0;
	for (i = 0; i < nonzeros; i++) {
		for (j = 0; j < order; j++) {
			k = indices[pos++];
			int now = tempX[j*max_dim + k];
			WhereX[now] = i;
			tempX[j*max_dim + k]++;
		}
	}
	free(tempX);
}

void partially_observed_pre_process(double *pre_check, int order, int rrank, int mult, float *FactorM, int* dim) {
	int i, j, k, l;
	for (i = 0; i < order; i++) {
		for (j = 0; j < rrank; j++) {
			for (k = 0; k < rrank; k++) {
				int pos1 = i*mult + j, pos2 = i*mult + k, now = i*rrank*rrank + j*rrank + k;
				pre_check[now] = 0;
				for (l = 0; l < dim[i]; l++) {
					pre_check[now] += FactorM[pos1 + l*rrank] * FactorM[pos2 + l*rrank];
				}
			}
		}
	}
}

void Computing_Delta(float* Delta, int rrank, int i) {
	int order = X.order, nnz = X.nonzeros, Core_N = CoreT.nonzeros, j, k, l, ii, jj;
	int *dim = X.dimension;
	int mult = max_dim*rrank;
	double st = omp_get_wtime(),st2=omp_get_wtime();
	if (X.gpu_mode != 0) {
			int NNZ_PER_DEVICE = nnz / num_devices + 1, last;
			if (nnz%num_devices == 0) NNZ_PER_DEVICE--;
			last = nnz - NNZ_PER_DEVICE*(num_devices - 1);
			cl_mem *bufA, *bufB, *bufC, *bufD, *bufE, *bufF;
			bufA = (cl_mem*)malloc(sizeof(cl_mem) * num_devices); bufB = (cl_mem*)malloc(sizeof(cl_mem) * num_devices); bufC = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);  bufD = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);  bufE = (cl_mem*)malloc(sizeof(cl_mem) * num_devices); bufF = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);
			for (int j = 0; j < num_devices; j++) {
				if (j != num_devices - 1) {
					bufA[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int) * NNZ_PER_DEVICE*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufA[j], CL_FALSE, 0, sizeof(int) * NNZ_PER_DEVICE*order, X.index + j*NNZ_PER_DEVICE*order, 0, NULL, NULL); CHECK_ERROR(err);
					bufB[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*NNZ_PER_DEVICE, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufB[j], CL_FALSE, 0, sizeof(float) * NNZ_PER_DEVICE, X.value + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
					bufF[j] = clCreateBuffer(context, CL_MEM_WRITE_ONLY, sizeof(float)*rrank*NNZ_PER_DEVICE, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufF[j], CL_FALSE, 0, sizeof(float)*NNZ_PER_DEVICE*rrank, (Delta + j*NNZ_PER_DEVICE*rrank), 0, NULL, NULL);
				}
				else {
					bufA[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int) * last*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufA[j], CL_FALSE, 0, sizeof(int) * last*order, X.index + j*NNZ_PER_DEVICE*order, 0, NULL, NULL); CHECK_ERROR(err);
					bufB[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*last, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufB[j], CL_FALSE, 0, sizeof(float) * last, X.value + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
					bufF[j] = clCreateBuffer(context, CL_MEM_WRITE_ONLY, sizeof(float)*rrank*last, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufF[j], CL_FALSE, 0, sizeof(float)*last*rrank, (Delta + j*NNZ_PER_DEVICE*rrank), 0, NULL, NULL);
				}
				bufC[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int)*Core_N*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufC[j], CL_FALSE, 0, sizeof(int) * Core_N*order, CoreT.index, 0, NULL, NULL); CHECK_ERROR(err);
				bufD[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*Core_N, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufD[j], CL_FALSE, 0, sizeof(float) * Core_N, CoreT.value, 0, NULL, NULL); CHECK_ERROR(err);
				bufE[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*order*max_dim*rrank, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufE[j], CL_FALSE, 0, sizeof(float) * order*max_dim*rrank, X.FactorM, 0, NULL, NULL); CHECK_ERROR(err);

			}

			for(int j=0;j<num_devices;j++){
					clFinish(queues[j]);
			}
			printf("CPU->GPU COPY TIME: %lf\n",omp_get_wtime()-st);
			copy_time += omp_get_wtime()-st;
			st = omp_get_wtime();

			for (int j = 0; j < num_devices; j++) {
				err = clSetKernelArg(kernels[j], 0, sizeof(cl_mem), &bufA[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 1, sizeof(cl_mem), &bufB[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 2, sizeof(cl_mem), &bufC[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 3, sizeof(cl_mem), &bufD[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 4, sizeof(cl_mem), &bufE[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 5, sizeof(cl_mem), &bufF[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 6, sizeof(cl_int), &order); CHECK_ERROR(err);
				if (j == num_devices - 1) {
					err = clSetKernelArg(kernels[j], 7, sizeof(cl_int), &last); CHECK_ERROR(err);
				}
				else {
					err = clSetKernelArg(kernels[j], 7, sizeof(cl_int), &NNZ_PER_DEVICE); CHECK_ERROR(err);
				}
				err = clSetKernelArg(kernels[j], 8, sizeof(cl_int), &rrank); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 9, sizeof(cl_int), &i); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 10, sizeof(cl_int), &Core_N); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 11, sizeof(cl_int), &mult); CHECK_ERROR(err);
			}
			size_t global_size = NNZ_PER_DEVICE;
			size_t local_size = X.local_size;
			global_size = (global_size + local_size - 1) / local_size * local_size;
			for (int j = 0; j < num_devices; j++) {
				err = clEnqueueNDRangeKernel(queues[j], kernels[j], 1, NULL, &global_size, &local_size, 0, NULL, NULL); CHECK_ERROR(err);
			}

			for(int j=0;j<num_devices;j++){
					clFinish(queues[j]);
			}
			printf("GPU COMPUTING TIME: %lf\n",omp_get_wtime()-st);
			gpu_time += omp_get_wtime()-st;
			st = omp_get_wtime();

			for (int j = 0; j < num_devices; j++) {
				if (j == num_devices - 1) {
					err = clEnqueueReadBuffer(queues[j], bufF[j], CL_TRUE, 0, sizeof(float)*last*rrank, Delta + j*NNZ_PER_DEVICE*rrank, 0, NULL, NULL); CHECK_ERROR(err);
				}
				else {
					err = clEnqueueReadBuffer(queues[j], bufF[j], CL_TRUE, 0, sizeof(float)*NNZ_PER_DEVICE*rrank, Delta + j*NNZ_PER_DEVICE*rrank, 0, NULL, NULL); CHECK_ERROR(err);
				}
			}

			for(int j=0;j<num_devices;j++){
					clFinish(queues[j]);
			}
			printf("GPU->CPU COPY TIME: %lf\n",omp_get_wtime()-st);
			copy_time += omp_get_wtime()-st;
			st = omp_get_wtime();

			for (int j = 0; j < num_devices; j++) {
				clReleaseMemObject(bufA[j]); clReleaseMemObject(bufB[j]);  clReleaseMemObject(bufC[j]);  clReleaseMemObject(bufD[j]); clReleaseMemObject(bufE[j]); clReleaseMemObject(bufF[j]);
			}
			free(bufA); free(bufB); free(bufC); free(bufD); free(bufE); free(bufF);
		}

	else {
		#pragma omp parallel for schedule(static)
		for (j = 0; j < nnz; j++) {
			int pre_val = j*order, pre_val2 = j*rrank, k, l, ii;
			int *cach1 = (int *)malloc(sizeof(int)*order);
			for (l = 0; l < order; l++) cach1[l] = X.index[pre_val + l];
			for (l = 0; l < rrank; l++) Delta[pre_val2 + l] = 0;
			for (l = 0; l < CoreT.nonzeros; l++) {
				int pre1 = l*order, pre2 = 0;
				int CorePos = CoreT.index[pre1 + i];
				double res = CoreT.value[l];
				for (ii = 0; ii < order; ii++) {
					if (ii != i) {
						int mulrow = cach1[ii], mulcol = CoreT.index[pre1];
						res *= X.FactorM[pre2 + mulrow*rrank + mulcol];
					}
					pre1++;
					pre2 += mult;
				}
				Delta[pre_val2 + CorePos] += res;
			}
			free(cach1);
		}
	}
	printf("Elapsed time for Computing Delta: %lf\n", omp_get_wtime() - st2);
}

void Computing_BC(float* Delta, double* B, double* C, int rank, int i) {
	int iter, row_size = X.dimension[i],mult = max_dim*rank;
	double st = omp_get_wtime();
	//Initialize B and C
	#pragma omp parallel for schedule(static) //in parallel
	for (int j = 0; j < row_size; j++) {
		int pos_B = j*rank*rank, pos_C = j*rank,k,l;
		for (k = 0; k < rank; k++) {
			for (l = 0; l < rank; l++) {
				B[pos_B] = 0;
				if (k == l) B[pos_B] = lambda;
				pos_B++;
			}
			C[pos_C++] = 0;
		}
	}

	#pragma omp parallel for schedule(dynamic) //in parallel
	for (iter = 0; iter < row_size; iter++) {
		int j=iter, k, l, ii, jj,pos_B,pos_C;
		int pos = i*max_dim + j;
		int nnz = CountX[pos + 1] - CountX[pos];
		pos = CountX[pos];
		for (k = 0; k < nnz; k++) { //Updating Delta, B, and C
			int current_input_entry = WhereX[pos + k];
			int pre_val = current_input_entry*rank;
			int now = 0;
			double Entry_val = X.value[current_input_entry];
			pos_B = j*rank*rank, pos_C = j*rank;
			for (ii = 0; ii < rank; ii++) {
				double cach = Delta[pre_val + ii];
				for (jj = 0; jj < rank; jj++) {
					B[pos_B++] += cach * Delta[pre_val + jj];
				}
				C[pos_C++] += cach * Entry_val;
			}
		}
	}
	printf("Elapsed time for Computing B and C: %lf\n", omp_get_wtime() - st);
}

void Update_Factor_Matrices(int rrank) {
	int order = X.order, nnz = X.nonzeros, Core_N = CoreT.nonzeros, i, j, k, l;
	int *dim = X.dimension;
	int *crows, rowcount, Core_dim;
	int mult = max_dim*rrank;
	double st = omp_get_wtime();
	float *Delta = (float *)malloc(sizeof(float)*nnz*rrank);
	double *pre_check = (double *)malloc(sizeof(double)*order*rrank*rrank);
	double *B = (double *)malloc(sizeof(double)*max_dim*rrank*rrank);
	double *C = (double *)malloc(sizeof(double)*max_dim*rrank);
	double *Shared_B = (double *)malloc(sizeof(double)*rrank*rrank);
	if (X.partially_observed == 0) {
		partially_observed_pre_process(pre_check, order, rrank, mult, X.FactorM, dim);
	}

	kernels = TF.kernels; queues = TF.queues; context = TF.context;
	printf("INIT TIME: %lf\n", omp_get_wtime() - st);

	copy_time = gpu_time = 0;

	for (i = 0; i < order; i++) { //Updating the ith Factor Matrix
		int row_size = dim[i];
		int column_size = rrank;
		int iter;

		st = omp_get_wtime();
		Computing_Delta(Delta, rrank, i);

		long long sizee = Core_N*Core_N;
		if (X.partially_observed == 0) {
			double st2 = omp_get_wtime();
			for (j = 0; j < column_size; j++) {
				for (k = 0; k < column_size; k++) {
					double temp = 0;
					if (j == k) temp = lambda;
					Shared_B[j*column_size + k] = temp;
				}
			}
			long long j;
			for (j = 0; j < sizee; j++) {
				int alpha = j / Core_N, beta = j - alpha*Core_N, k, aa;
				double totalval = CoreT.value[alpha] * CoreT.value[beta];
				for (k = 0; k < order; k++) {
					if (k != i) {
						int pos3 = CoreT.index[alpha*order + k], pos4 = CoreT.index[beta*order + k], pos5 = k*mult;
						totalval *= pre_check[k*rrank*rrank + pos3*rrank + pos4];
					}
				}
				int pos1 = CoreT.index[alpha*order + i], pos2 = CoreT.index[beta*order + i];
				Shared_B[pos1*column_size + pos2] += totalval;
			}
			printf("Shared_B calculation time : %lf\n", omp_get_wtime() - st2);
		}

		Computing_BC(Delta, B, C, rrank, i);

		double st2 = omp_get_wtime();
		#pragma omp parallel for schedule(static) //in parallel
		for (iter = 0; iter < row_size; iter++) {
			//Getting the inverse matrix of [B+lambda*I]
			int pos, j, k, l;
			j = iter;
			int pos_B = j*column_size*column_size, pos_C = j*column_size;
			MatrixXdd AA(column_size, column_size);
			pos = 0;
			for (k = 0; k < column_size; k++) {
				for (l = 0; l < column_size; l++) {
					if (X.partially_observed == 1) AA(k, l) = B[pos_B+k*column_size + l];
					else AA(k, l) = Shared_B[k*column_size + l];
				}
			}

			MatrixXdd BB = AA.inverse();

			//Update the jth row of ith Factor Matrix
			int cach = i*mult + j*column_size;
			for (k = 0; k < column_size; k++) {
				double res = 0;
				for (l = 0; l < column_size; l++) {
					res += C[pos_C + l] * BB(l , k);
				}
				X.FactorM[cach + k] = res;
			}
			AA.resize(0,0); BB.resize(0,0);
		}

		printf("UPDATE TIME : %lf\n",omp_get_wtime()-st2);
		if (X.partially_observed == 0) {
			for (j = 0; j < rrank; j++) {
				for (k = 0; k < rrank; k++) {
					int pos1 = i*mult + j, pos2 = i*mult + k, now = i*rrank*rrank + j*rrank + k;
					pre_check[now] = 0;
					for (l = 0; l < dim[i]; l++) {
						double a1 =  X.FactorM[pos1 + l*rrank], a2 =   X.FactorM[pos2 + l*rrank];
						pre_check[now] += a1*a2;
					}
				}
			}
		}

		printf("[FACTOR MATRIX %d] UPDATE time : %lf\n\n", i + 1, (omp_get_wtime() - st));

	}
	printf("copy time: %lf\tgpu time: %lf\n",copy_time,gpu_time);

	free(Delta); free(pre_check); free(Shared_B); free(B); free(C);
}

void Reconstruction(int rrank) {
	int order = X.order, nnz = X.nonzeros, Core_N = CoreT.nonzeros, i, j, k, l;
	int* dim = X.dimension;
	float *Error_T, Error = 0, NormX = 0;
	Error_T = (float *)malloc(sizeof(float)*nnz);
	double st = omp_get_wtime();
	int mult = max_dim*rrank;

	if (X.gpu_mode == 0) {
		#pragma omp parallel for schedule(static)
		for (i = 0; i < nnz; i++) {
			int j, pre_val = i*order;
			double ans = 0;
			int *cach1 = (int *)malloc(sizeof(int)*order);
			for (j = 0; j < order; j++) cach1[j] = X.index[pre_val++];
			for (j = 0; j < Core_N; j++) {
				double temp = CoreT.value[j];
				int k;
				int pos = j*order;
				int val = 0;
				for (k = 0; k < order; k++) {
					int mulrow = cach1[k], mulcol = CoreT.index[pos++];
					temp *= X.FactorM[val + mulrow*rrank + mulcol];
					val += mult;
				}
				ans += temp;
			}
			free(cach1);
			Error_T[i] = ans;
		}
	}

	if (X.gpu_mode != 0) {

		kernels = RECON.kernels; queues = RECON.queues; context = RECON.context;

		int NNZ_PER_DEVICE = nnz / num_devices + 1, last;
			if (nnz%num_devices == 0) NNZ_PER_DEVICE--;
			last = nnz - NNZ_PER_DEVICE*(num_devices - 1);
			cl_mem *bufA, *bufB, *bufC, *bufD, *bufE, *bufF;
			bufA = (cl_mem*)malloc(sizeof(cl_mem) * num_devices); bufB = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);  bufC = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);  bufD = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);  bufE = (cl_mem*)malloc(sizeof(cl_mem) * num_devices); bufF = (cl_mem*)malloc(sizeof(cl_mem) * num_devices);
			for (int j = 0; j < num_devices; j++) {
				if (j != num_devices - 1) {
					bufA[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int) * NNZ_PER_DEVICE*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufA[j], CL_FALSE, 0, sizeof(int) * NNZ_PER_DEVICE*order, X.index + j*NNZ_PER_DEVICE*order, 0, NULL, NULL); CHECK_ERROR(err);
					bufB[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*NNZ_PER_DEVICE, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufB[j], CL_FALSE, 0, sizeof(float) * NNZ_PER_DEVICE, X.value + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
					bufF[j] = clCreateBuffer(context, CL_MEM_READ_WRITE, sizeof(float)*NNZ_PER_DEVICE, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufF[j], CL_FALSE, 0, sizeof(float)*NNZ_PER_DEVICE, (Error_T + j*NNZ_PER_DEVICE), 0, NULL, NULL);
				}
				else {
					bufA[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int) * last*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufA[j], CL_FALSE, 0, sizeof(int) * last*order, X.index + j*NNZ_PER_DEVICE*order, 0, NULL, NULL); CHECK_ERROR(err);
					bufB[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*last, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufB[j], CL_FALSE, 0, sizeof(float) * last, X.value + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
					bufF[j] = clCreateBuffer(context, CL_MEM_READ_WRITE, sizeof(float)*last, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufF[j], CL_FALSE, 0, sizeof(float)*last, (Error_T + j*NNZ_PER_DEVICE), 0, NULL, NULL);
				}
				bufC[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(int)*Core_N*order, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufC[j], CL_FALSE, 0, sizeof(int) * Core_N*order, CoreT.index, 0, NULL, NULL); CHECK_ERROR(err);
				bufD[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*Core_N, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufD[j], CL_FALSE, 0, sizeof(float) * Core_N, CoreT.value, 0, NULL, NULL); CHECK_ERROR(err);
				bufE[j] = clCreateBuffer(context, CL_MEM_READ_ONLY, sizeof(float)*order*max_dim*rrank, NULL, &err); CHECK_ERROR(err); err = clEnqueueWriteBuffer(queues[j], bufE[j], CL_FALSE, 0, sizeof(float) * order*max_dim*rrank, X.FactorM, 0, NULL, NULL); CHECK_ERROR(err);

			}

			for (int j = 0; j < num_devices; j++) {
				err = clSetKernelArg(kernels[j], 0, sizeof(cl_mem), &bufA[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 1, sizeof(cl_mem), &bufB[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 2, sizeof(cl_mem), &bufC[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 3, sizeof(cl_mem), &bufD[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 4, sizeof(cl_mem), &bufE[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 5, sizeof(cl_mem), &bufF[j]); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 6, sizeof(cl_int), &order); CHECK_ERROR(err);
				if (j == num_devices - 1) {
					err = clSetKernelArg(kernels[j], 7, sizeof(cl_int), &last); CHECK_ERROR(err);
				}
				else {
					err = clSetKernelArg(kernels[j], 7, sizeof(cl_int), &NNZ_PER_DEVICE); CHECK_ERROR(err);
				}
				err = clSetKernelArg(kernels[j], 8, sizeof(cl_int), &rrank); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 9, sizeof(cl_int), &Core_N); CHECK_ERROR(err);
				err = clSetKernelArg(kernels[j], 10, sizeof(cl_int), &mult); CHECK_ERROR(err);
			}

			size_t global_size = NNZ_PER_DEVICE;
			size_t local_size = X.local_size/2;
			global_size = (global_size + local_size - 1) / local_size * local_size;

			for (int j = 0; j < num_devices; j++) {
				err = clEnqueueNDRangeKernel(queues[j], kernels[j], 1, NULL, &global_size, &local_size, 0, NULL, NULL); CHECK_ERROR(err);
			}

			for (int j = 0; j < num_devices; j++) {
				if (j == num_devices - 1) {
					err = clEnqueueReadBuffer(queues[j], bufF[j], CL_TRUE, 0, sizeof(float)*last, Error_T + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
				}
				else {
					err = clEnqueueReadBuffer(queues[j], bufF[j], CL_TRUE, 0, sizeof(float)*NNZ_PER_DEVICE, Error_T + j*NNZ_PER_DEVICE, 0, NULL, NULL); CHECK_ERROR(err);
				}
			}

			for (int j = 0; j < num_devices; j++) {
				clReleaseMemObject(bufA[j]); clReleaseMemObject(bufB[j]); clReleaseMemObject(bufC[j]); clReleaseMemObject(bufD[j]); clReleaseMemObject(bufE[j]); clReleaseMemObject(bufF[j]);
			}
			free(bufA); free(bufB); free(bufC); free(bufD); free(bufE); free(bufF);
		}

	if (X.partially_observed == 0) {
		double *pre_check = (double *)malloc(sizeof(double)*order*rrank*rrank);

		partially_observed_pre_process(pre_check, order, rrank, mult, X.FactorM, dim);

		long long sizee = Core_N*Core_N,j;
		for (j = 0; j < sizee; j++) {
			int alpha = j / Core_N, beta = j - alpha*Core_N, k, aa;
			double totalval = CoreT.value[alpha] * CoreT.value[beta];
			for (k = 0; k < order; k++) {
				int pos3 = CoreT.index[alpha*order + k], pos4 = CoreT.index[beta*order + k], pos5 = k*mult;
				totalval *= pre_check[k*rrank*rrank + pos3*rrank + pos4];
			}
			Error+=totalval;
		}
		free(pre_check);

#pragma omp parallel for schedule(static)
		for (i = 0; i < nnz; i++) {
			float temp = X.value[i] * (X.value[i] - 2 * Error_T[i]);
			Error_T[i] = temp;
		}
	}
	st = omp_get_wtime();
	if (X.partially_observed == 1) {
#pragma omp parallel for schedule(static) reduction(+:Error)
		for (i = 0; i < nnz; i++) {
			Error += (X.value[i] - Error_T[i]) * (X.value[i] - Error_T[i]);
		}
	}
	else {
#pragma omp parallel for schedule(static) reduction(+:Error)
		for (i = 0; i < nnz; i++) {
			Error += Error_T[i];
		}
	}

#pragma omp parallel for schedule(static) reduction(+:NormX)
	for (i = 0; i < nnz; i++) {
		NormX += X.value[i] * X.value[i];
	}

	NormX = sqrt(NormX);
	if (NormX == 0) Fit = 1;
	else Fit = 1 - sqrt(Error) / NormX;
	free(Error_T);
}

void tensor_factorization(int rrank, char* Path) {
	printf("INITIALIZING FOR TF......\n");
	int order = X.order, nnz = X.nonzeros, i, j, k, threads = omp_get_max_threads();
	omp_set_num_threads(threads);
	num_devices = X.gpu_mode;

	//INITIALIZE
	for(i=0;i<order;i++){
		if(max_dim<X.dimension[i]) max_dim = X.dimension[i];
	}
	max_dim++;

	X.FactorM = (float *)malloc(sizeof(float)*order*max_dim * rrank);
	for (i = 0; i < order; i++) {
		int row = X.dimension[i];
		for (j = 0; j < row; j++) {
			for (k = 0; k < rrank; k++) {
				X.FactorM[i*max_dim*rrank + j*rrank + k] = ffrand(0, 1);
			}
		}
	}
	int Core_N = 1;
	int* core_dim = (int *)malloc(sizeof(int)*order);
	for (i = 0; i < order; i++) {
		core_dim[i] = rrank;
		Core_N *= rrank;
	}
	int* core_index = (int *)malloc(sizeof(int)*order*Core_N);
	float* core_val = (float *)malloc(sizeof(float)*order*Core_N);
	for (i = 0; i < Core_N; i++) {
		core_val[i] = ffrand(0, 1);
		if (i == 0) {
			for (j = 0; j < order; j++) core_index[j] = 0;
		}
		else {
			for (j = 0; j < order; j++) {
				core_index[i*order + j] = core_index[(i - 1)*order + j];
			}
			core_index[i*order + order - 1]++;  k = order - 1;
			while (core_index[i*order + k] >= rrank) {
				core_index[i*order + k] -= rrank;
				core_index[i*order + k - 1]++; k--;
			}
		}
	}

	Construct_Core_Tensor(order, Core_N, core_dim, core_index, core_val);

	WhereX = (int *)malloc(sizeof(int)*order*nnz);
	CountX = (int *)malloc(sizeof(int)*max_dim*order);
	assign_index();
	TF = GPU_INIT(X.gpu_mode, "src/GTA_GPU_Delta.cl", "compute_delta");
	RECON = GPU_INIT(X.gpu_mode, "src/GTA_reconstruction.cl", "recon");

	//Main Iteration
	int iter = 0;
	double avertime = omp_get_wtime();
	printf("INITIALIZE DONE!\n\nTensor order: %d\tnonzeros: %d\trrank: %d\tthreads: %d\tLocal size: %d\tNumber of GPUs: %d\n\n", order, nnz, rrank, omp_get_max_threads(),X.local_size,num_devices);
	if (X.gpu_mode == 0 && X.partially_observed==0) printf("Starting Tucker Factorization for a Fully Observable Tensor in CPU-mode......\n\n");
	if (X.gpu_mode == 0 && X.partially_observed==1) printf("Starting Tucker Factorization for a Partially Observable Tensor in CPU-mode......\n\n");
	if (X.gpu_mode == 1 && X.partially_observed==0) printf("Starting Tucker Factorization for a Fully Observable Tensor in Single-GPU-mode......\n\n");
	if (X.gpu_mode == 1 && X.partially_observed==1) printf("Starting Tucker Factorization for a Partially Observable Tensor in Single-GPU-mode......\n\n");
	if (X.gpu_mode >= 2 && X.partially_observed==0) printf("Starting Tucker Factorization for a Fully Observable Tensor in Multi-GPU-mode......\n\n");
	if (X.gpu_mode >= 2 && X.partially_observed==1) printf("Starting Tucker Factorization for a Partially Observable Tensor in Multi-GPU-mode......\n\n");

	while (1) {

		double itertime = omp_get_wtime(), steptime;
		steptime = itertime;

		Update_Factor_Matrices(rrank);
		printf("Factor Time : %lf\n", omp_get_wtime() - steptime);
		steptime = omp_get_wtime();

		Reconstruction(rrank);
		printf("Recon Time : %lf\n\n", omp_get_wtime() - steptime);
		steptime = omp_get_wtime();

		printf("iter%d :      Fit : %lf\tElapsed Time : %lf\n\n", ++iter, Fit, omp_get_wtime() - itertime);

		if (iter>=10 || (pFit != -1 && abss(pFit - Fit) <= 0.0001)) break;
		pFit = Fit;
	}
	printf("Average Elapsed time per iteration: %lf\n\n", (omp_get_wtime() - avertime) / iter);

	printf("\nWriting factor matrices and core tensor to file...\n");
	char temp[10005];
	int pos = 0;
	int mult = max_dim*rrank;
	for (i = 0; i < order; i++) {
		sprintf(temp, "%s/FACTOR%d", Path, i);
		FILE *fin = fopen(temp, "w");
		for (j = 0; j < X.dimension[i]; j++) {
			for (k = 0; k < rrank; k++) {
				fprintf(fin, "%e\t", X.FactorM[i*mult + j*rrank + k]);
			}
			fprintf(fin, "\n");
		}
	}
	sprintf(temp, "%s/CORETENSOR", Path);
	FILE *fcore = fopen(temp, "w");
	pos = 0;
	for (i = 0; i < Core_N; i++) {
		for (j = 0; j < order; j++) {
			fprintf(fcore, "%d\t", CoreT.index[pos++]);
		}
		fprintf(fcore, "%e\n", CoreT.value[i]);
	}

	GPU_DONE(X.gpu_mode,TF);
	GPU_DONE(X.gpu_mode, RECON);
}

int main(int argc, char* argv[]) {

	if (argc !=0) {
		InputPath = argv[1];
		ResultPath = argv[2];
		rrank = atoi(argv[3]);
		l_size = atoi(argv[4]);
		gpu_mode = atoi(argv[5]);
		nnz_mode = atoi(argv[6]);

		Read_Tensor(InputPath);

		tensor_factorization(rrank,ResultPath);
	}

	return 0;
}