#include "dynamic_depth/pose.h"

#include <math.h>

#include <cmath>

#include "android-base/logging.h"
#include "dynamic_depth/const.h"

using ::dynamic_depth::xmpmeta::xml::Deserializer;
using ::dynamic_depth::xmpmeta::xml::Serializer;

namespace dynamic_depth {
namespace {

const char kPositionX[] = "PositionX";
const char kPositionY[] = "PositionY";
const char kPositionZ[] = "PositionZ";
const char kRotationX[] = "RotationX";
const char kRotationY[] = "RotationY";
const char kRotationZ[] = "RotationZ";
const char kRotationW[] = "RotationW";
const char kTimestamp[] = "Timestamp";
const char kNamespaceHref[] = "http://ns.google.com/photos/dd/1.0/pose/";

const std::vector<float> NormalizeQuaternion(const std::vector<float>& quat) {
  if (quat.size() < 4) {
    return std::vector<float>();
  }
  float length = std::sqrt((quat[0] * quat[0]) + (quat[1] * quat[1]) +
                           (quat[2] * quat[2]) + (quat[3] * quat[3]));
  const std::vector<float> normalized = {quat[0] / length, quat[1] / length,
                                         quat[2] / length, quat[3] / length};
  return normalized;
}

}  // namespace

// Private constructor.
Pose::Pose() : timestamp_(-1) {}

// Public methods.
void Pose::GetNamespaces(std::unordered_map<string, string>* ns_name_href_map) {
  if (ns_name_href_map == nullptr) {
    LOG(ERROR) << "Namespace list or own namespace is null";
    return;
  }
  ns_name_href_map->emplace(DynamicDepthConst::Pose(), kNamespaceHref);
}

std::unique_ptr<Pose> Pose::FromData(const std::vector<float>& position,
                                     const std::vector<float>& orientation,
                                     const int64 timestamp) {
  if (position.empty() && orientation.empty()) {
    LOG(ERROR) << "Either position or orientation must be provided";
    return nullptr;
  }

  std::unique_ptr<Pose> pose(new Pose());
  if (position.size() >= 3) {
    pose->position_ = position;
  }

  if (orientation.size() >= 4) {
    pose->orientation_ = NormalizeQuaternion(orientation);
  }

  if (timestamp >= 0) {
    pose->timestamp_ = timestamp;
  }

  return pose;
}

std::unique_ptr<Pose> Pose::FromDeserializer(
    const Deserializer& parent_deserializer, const char* parent_namespace) {
  std::unique_ptr<Deserializer> deserializer =
      parent_deserializer.CreateDeserializer(parent_namespace,
                                             DynamicDepthConst::Pose());
  if (deserializer == nullptr) {
    return nullptr;
  }
  std::unique_ptr<Pose> pose(new Pose());
  if (!pose->ParsePoseFields(*deserializer)) {
    return nullptr;
  }
  return pose;
}

bool Pose::HasPosition() const { return position_.size() == 3; }
bool Pose::HasOrientation() const { return orientation_.size() == 4; }

const std::vector<float>& Pose::GetPosition() const { return position_; }

const std::vector<float>& Pose::GetOrientation() const { return orientation_; }

int64 Pose::GetTimestamp() const { return timestamp_; }

bool Pose::Serialize(Serializer* serializer) const {
  if (serializer == nullptr) {
    LOG(ERROR) << "Serializer is null";
    return false;
  }

  if (!HasPosition() && !HasOrientation()) {
    LOG(ERROR) << "Camera pose has neither position nor orientation";
    return false;
  }

  bool success = true;
  if (position_.size() == 3) {
    success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionX,
                                         std::to_string(position_[0])) &&
               serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionY,
                                         std::to_string(position_[1])) &&
               serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionZ,
                                         std::to_string(position_[2]));
  }

  if (orientation_.size() == 4) {
    success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationX,
                                         std::to_string(orientation_[0])) &&
               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationY,
                                         std::to_string(orientation_[1])) &&
               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationZ,
                                         std::to_string(orientation_[2])) &&
               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationW,
                                         std::to_string(orientation_[3]));
  }

  if (timestamp_ >= 0) {
    serializer->WriteProperty(DynamicDepthConst::Pose(), kTimestamp,
                              std::to_string(timestamp_));
  }

  return success;
}

// Private methods.
bool Pose::ParsePoseFields(const Deserializer& deserializer) {
  float x, y, z, w;
  const string& prefix = DynamicDepthConst::Pose();
  // If a position field is present, the rest must be as well.
  if (deserializer.ParseFloat(prefix, kPositionX, &x)) {
    if (!deserializer.ParseFloat(prefix, kPositionY, &y)) {
      return false;
    }
    if (!deserializer.ParseFloat(prefix, kPositionZ, &z)) {
      return false;
    }
    position_ = {x, y, z};
  }

  // Same for orientation.
  if (deserializer.ParseFloat(prefix, kRotationX, &x)) {
    if (!deserializer.ParseFloat(prefix, kRotationY, &y)) {
      return false;
    }
    if (!deserializer.ParseFloat(prefix, kRotationZ, &z)) {
      return false;
    }
    if (!deserializer.ParseFloat(prefix, kRotationW, &w)) {
      return false;
    }
    orientation_ = std::vector<float>({x, y, z, w});
  }

  if (position_.size() < 3 && orientation_.size() < 4) {
    return false;
  }

  deserializer.ParseLong(prefix, kTimestamp, &timestamp_);
  return true;
}

}  // namespace dynamic_depth