//
// Copyright (c) 2009 Froduald Kabanza and the Université de Sherbrooke.
// Use of this software is permitted for non-commercial research purposes, and
// it may be copied or applied only for that use. All copies must include this
// copyright message.
// 
// This is a research prototype and it has not gone through intensive tests and
// is delivered as is. It may still contain bugs. Froduald Kabanza and the
// Université de Sherbrooke disclaim any responsibility for damage that may be
// caused by using it.
//
// Implementation: Simon Chamberland
// Project Manager: Froduald Kabanza
//

#include "stdafx.h"
#include "QualifiedWorld.h"
#include "../../../Exp/Formula/Described/AtomicFormulaApplication.h"
#include "../../../Exp/Formula/Described/NumericFluentApplication.h"
#include "../../../Exp/Formula/Described/ObjectFluentApplication.h"
#include "../../../Exception/UndefinedFluentException.h"
#include "../../../Exception/NumericException.h"

#include <assert.h>

using namespace planning;

QualifiedWorld::QualifiedWorld()
  : m_hashCode(0)
{
}

QualifiedWorld::QualifiedWorld(const QualifiedWorld &world)
  : m_facts(world.m_facts), m_numericFluents(world.m_numericFluents), 
    m_objectFluents(world.m_objectFluents), m_hashCode(0)
{
}

QualifiedWorld* QualifiedWorld::copy() const
{
  return new QualifiedWorld(*this);
}

bool QualifiedWorld::isSet(const AtomicFormulaApplication* formula) const
{
  return m_facts.find(formula) != m_facts.end();
}

void QualifiedWorld::set(const AtomicFormulaApplication* formula)
{
  std::pair<std::set<const AtomicFormulaApplication*>::iterator, bool> p = 
    m_facts.insert(formula);

  // Update hash code if formula was not present before
  if (p.second)
  {
    m_hashCode += formula->getHashCode();
  }
}

void QualifiedWorld::unset(const AtomicFormulaApplication* formula)
{
  if (m_facts.erase(formula) != 0)
  {
    m_hashCode -= formula->getHashCode();
  }
}

double QualifiedWorld::getNumericFluent(const NumericFluentApplication* fluent) const
{
  std::map<const NumericFluentApplication*, double>::const_iterator it = 
    m_numericFluents.find(fluent);

  if (it == m_numericFluents.end())
    throw UndefinedFluentException(fluent);

  return it->second;
}

const Object* QualifiedWorld::getObjectFluent(const ObjectFluentApplication* fluent) const
{
  std::map<const ObjectFluentApplication*, const Object*>::const_iterator it = 
    m_objectFluents.find(fluent);

  if (it == m_objectFluents.end())
    throw UndefinedFluentException(fluent);

  return it->second;
}

void QualifiedWorld::setNumericFluent(const NumericFluentApplication* fluent, double value)
{
  if ( !(boost::math::isfinite)(value) )
    throw NumericException(fluent, value);

  std::pair<std::map<const NumericFluentApplication*, double>::iterator, bool> p =
    m_numericFluents.insert(std::make_pair(fluent, value));

  const size_t hashId = fluent->getHashCode();
  double &oldValue = p.first->second;
  if (!p.second)
  {
    m_hashCode -= (hashId * m_doubleHasher(oldValue));
    oldValue = value;
  }
  m_hashCode += (hashId * m_doubleHasher(oldValue));
}

void QualifiedWorld::setNumericFluent(const NumericFluentApplication* fluent, 
                                      const ArithmeticFunction& function, 
                                      double argument)
{
  std::map<const NumericFluentApplication*, double>::iterator it = 
    m_numericFluents.find(fluent);

  if (it == m_numericFluents.end())
  {
    // Cannot use a function with an undefined fluent
    throw UndefinedFluentException(fluent);
  }
  const size_t hashId = fluent->getHashCode();
  double &value = it->second;
  m_hashCode -= (hashId * m_doubleHasher(value));
  value = function(value, argument);
  if ( !(boost::math::isfinite)(value) )
    throw NumericException(fluent, value);

  m_hashCode += (hashId * m_doubleHasher(value));
}

void QualifiedWorld::setObjectFluent(const ObjectFluentApplication* fluent, const Object* value)
{
  std::pair<std::map<const ObjectFluentApplication*, const Object*>::iterator, bool> p =
    m_objectFluents.insert(std::make_pair(fluent, value));

  const size_t hashId = fluent->getHashCode();
  const Object* &oldValue = p.first->second;
  if (!p.second)
  {
    m_hashCode -= (hashId * oldValue->getHashCode());
    oldValue = value;
  }
  m_hashCode += (hashId * oldValue->getHashCode());
}

void QualifiedWorld::undefineObjectFluent(const ObjectFluentApplication* fluent)
{
  std::map<const ObjectFluentApplication*, const Object*>::iterator it = 
    m_objectFluents.find(fluent);

  if (it != m_objectFluents.end())
  {
    const size_t hashId = fluent->getHashCode();
    m_hashCode -= (hashId * it->second->getHashCode());
    m_objectFluents.erase(it);
  }
}

size_t QualifiedWorld::getHashCode() const
{
  return m_hashCode;
}

void QualifiedWorld::_output(std::ostream& out) const
{
  // TODO
  out << "QualifiedWorld";
}

int QualifiedWorld::compareTo(const IClosedWorld& other) const
{
  const QualifiedWorld& world = static_cast<const QualifiedWorld&>(other);

  COMPARE(m_hashCode, world.m_hashCode);

  COMPARE(m_facts.size(), world.m_facts.size());

  COMPARE(m_numericFluents.size(), world.m_numericFluents.size());

  COMPARE(m_objectFluents.size(), world.m_objectFluents.size());

  for (std::set<const AtomicFormulaApplication*>::const_iterator 
        it1 = m_facts.begin(), 
        it2 = world.m_facts.begin();
       it1 != m_facts.end();
       ++it1, ++it2)
  {
    COMPARE(*it1, *it2);
  }

  for (std::map<const NumericFluentApplication*, double>::const_iterator 
        it1 = m_numericFluents.begin(), 
        it2 = world.m_numericFluents.begin();
       it1 != m_numericFluents.end();
       ++it1, ++it2)
  {
    COMPARE(it1->first, it2->first);
    COMPARE(it1->second, it2->second);
  }

  for (std::map<const ObjectFluentApplication*, const Object*>::const_iterator 
        it1 = m_objectFluents.begin(), 
        it2 = world.m_objectFluents.begin();
       it1 != m_objectFluents.end();
       ++it1, ++it2)
  {
    COMPARE(it1->first, it2->first);
    COMPARE(it1->second, it2->second);
  }

  return 0;
}