/***************************************************************************
 *   Copyright (C) 2004 by Raphael Langerhorst                             *
 *   raphael-langerhorst@gmx.at                                            *
 *                                                                         *
 *   Permission is hereby granted, free of charge, to any person obtaining *
 *   a copy of this software and associated documentation files (the       *
 *   "Software"), to deal in the Software without restriction, including   *
 *   without limitation the rights to use, copy, modify, merge, publish,   *
 *   distribute, sublicense, and/or sell copies of the Software, and to    *
 *   permit persons to whom the Software is furnished to do so, subject to *
 *   the following conditions:                                             *
 *                                                                         *
 *   The above copyright notice and this permission notice shall be        *
 *   included in all copies or substantial portions of the Software.       *
 *                                                                         *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       *
 *   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    *
 *   MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*
 *   IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR     *
 *   OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, *
 *   ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR *
 *   OTHER DEALINGS IN THE SOFTWARE.                                       *
 ***************************************************************************/

#include "GMoveAgent.h"

#include <GObject.h>
#include <GForm.h>
#include <GVector3.h>

#include <qmutex.h>
#include <qdom.h>

using namespace GCS;

namespace GBE {

GMoveAgent::GMoveAgent()
{
}


GMoveAgent::~GMoveAgent()
{
}

void GMoveAgent::run()
{
  if (requestObject()->hasForm())
  {
    bool existed;
    unsigned long interval = xmlGetULongInteger("/dynamics/updateinterval",existed);
    if (!existed)
    {
      qWarning("dynamics updateinterval did not exist!");
      interval = 1000;
      initUpdateInterval(interval);
    }
    Time.start();
    while(!shutdown)
    {
      updateForm();
      msleep(interval);
    }
  }
  else
  {
    qDebug(QString("element %1 has no GForm object, GMoveAgent is useless in this case").arg(getElementID().getID()));
  }
}

void GMoveAgent::updateForm()
{
  double delta_t = Time.restart()*0.001;
  GForm* f = requestForm();
  
  {
    
    bool ok;
    GVector3 TranslationSpeed = xmlGetVector3("/dynamics/translationspeed",ok);
    
    if (!ok)
    {
      TranslationSpeed.set(0,0,0);
      qWarning("Translation speed not found");
      initTranslationSpeed(TranslationSpeed);
    }
    
    GVector3 RotationSpeed = xmlGetVector3("/dynamics/rotationspeed",ok);
    if (!ok)
    {
      RotationSpeed.set(0,0,0);
      qWarning("Rotation speed not found, setting to (0,0,0)");
      initRotationSpeed(RotationSpeed);
    }
    
    double SlowDownFactor = xmlGetDouble("/dynamics/slowdownfactor",ok);
    if (!ok)
    {
      qWarning("dynamics slowdownfactor not found, setting to 1 - no slowdown");
      SlowDownFactor = 1;
      initSlowDownFactor(SlowDownFactor);
    }
  
    //slows down a bit
     
    TranslationSpeed.sub(TranslationSpeed*(SlowDownFactor*delta_t));
    RotationSpeed.sub(RotationSpeed*(SlowDownFactor*delta_t));
    
    f->lock();
    
    f->Position.add(TranslationSpeed * delta_t);
    
    f->Rotation.add(RotationSpeed * delta_t);
    
    f->unlock();
    
    xmlSetVector3("/dynamics/translationspeed",TranslationSpeed,ok);
    xmlSetVector3("/dynamics/rotationspeed",RotationSpeed,ok);
    
//     qDebug(QString("translation speed: %1, %2, %3").arg(QString::number(TranslationSpeed.x),QString::number(TranslationSpeed.y),QString::number(TranslationSpeed.z)));
//     qDebug(QString("new position: %1, %2, %3").arg(QString::number(f->Position.x),QString::number(f->Position.y),QString::number(f->Position.z)));
  }
  
  emit this->formChanged(*f);
}

void GMoveAgent::addTranslationSpeedImpulse(const GCS::GVector3& translation_impulse)
{
  bool ok;
  GVector3 speed = xmlGetVector3("/dynamics/translationspeed",ok);
  speed.add(translation_impulse);
  xmlSetVector3("/dynamics/translationspeed",speed,ok);
  emit this->agentChanged(*this);
}

void GMoveAgent::addRotationImpulse(const GCS::GVector3& rotation_impulse)
{
  bool ok;
  GVector3 speed = xmlGetVector3("/dynamics/rotationspeed",ok);
  speed.add(rotation_impulse);
  xmlSetVector3("/dynamics/rotationspeed",speed,ok);
  emit this->agentChanged(*this);
}

void GMoveAgent::fullStop()
{
  GVector3 null_vector(0,0,0);
  bool ok;
  xmlSetVector3("/dynamics/translationspeed",null_vector,ok);
  xmlSetVector3("/dynamics/rotationspeed",null_vector,ok);
  emit this->agentChanged(*this);
}

void GMoveAgent::initTranslationSpeed(const GCS::GVector3& speed)
{
  bool ok;
//   qDebug(QString("init translation speed: %1 %2 %3").arg(speed.x).arg(speed.y).arg(speed.z));
  xmlSetVector3("/dynamics/translationspeed",speed,ok);
//   GVector3 trs = xmlGetVector3("/dynamics/translationspeed",ok);
//   qDebug(QString("read translation speed: %1 %2 %3").arg(trs.x).arg(trs.y).arg(trs.z));
//   if (ok)
//     qDebug("ok is true");
//   else
//     qDebug("ok is false");
}

void GMoveAgent::initRotationSpeed(const GCS::GVector3& speed)
{
  bool ok;
  xmlSetVector3("/dynamics/rotationspeed",speed,ok);
}

void GMoveAgent::initSlowDownFactor(double factor)
{
  bool ok;
  xmlSetDouble("/dynamics/slowdownfactor",factor,ok);
}

void GMoveAgent::initUpdateInterval(unsigned long interval)
{
  bool ok;
  xmlSetULongInteger("/dynamics/updateinterval",interval,ok);
}

};