/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#if defined(_MSC_VER)
	#pragma warning(disable:4267)
#endif

#include "CHolonomicVFF.h"
#include <MRPT/MRML/CPoint2D.h>
using namespace UTILS;
using namespace MRML;
using namespace std;

using namespace MRPTAPPS::RNAV;

IMPLEMENTS_SERIALIZABLE( CLogFileRecord_VFF, CHolonomicLogFileRecord,MRPTAPPS::RNAV )

/*---------------------------------------------------------------
						initialize
  ---------------------------------------------------------------*/
CHolonomicVFF::CHolonomicVFF(const UTILS::CConfigFileBase *INI_FILE)
{
    if (INI_FILE!=NULL)
        initialize( *INI_FILE );
}


/*---------------------------------------------------------------
						initialize
  ---------------------------------------------------------------*/
void  CHolonomicVFF::initialize( const UTILS::CConfigFileBase &INI_FILE )
{

}

/*---------------------------------------------------------------
						navigate
  ---------------------------------------------------------------*/
void  CHolonomicVFF::navigate(
				MRML::CPoint2D	&target,
				vector_float	&obstacles,
				float			maxRobotSpeed,
				float			&desiredDirection,
				float			&desiredSpeed,
				CHolonomicLogFileRecord **logRecord)
{
	// Create a log record for returning data.
	if (logRecord) (*logRecord) = new CLogFileRecord_VFF();

        // Forces vector:
        MRML::CPoint2D  resultantForce,instantaneousForce;
        int             i,n=obstacles.size();
        float           ang, mod;

        // Obstacles:
        for (i=0;i<n;i++)
        {
                ang = (float)(M_PI *( -1 + 2*i/(float)n ));

                // Compute force strength:
                mod = exp(- obstacles[i] );

                // Add repulsive force:
                instantaneousForce.x = -cos(ang) * mod;
                instantaneousForce.y = -sin(ang) * mod;
                instantaneousForce.z = 0;
                resultantForce.AddComponents( instantaneousForce );
        }

        // Target:
        ang = atan2( target.y, target.x );
        mod = 30;
        instantaneousForce.x = cos(ang) * mod;
        instantaneousForce.y = sin(ang) * mod;
        resultantForce.AddComponents( instantaneousForce );

        // Result:
        desiredDirection = atan2( resultantForce.y, resultantForce.x );

	// Speed control: Reduction factors
	// ---------------------------------------------
	float		TARGET_SLOW_APPROACHING_DISTANCE = 0.05f;
	float		targetNearnessFactor = min( 1.0f, (float)( (sqrt(target.norm()) )/(2*TARGET_SLOW_APPROACHING_DISTANCE) ) );
	desiredSpeed = maxRobotSpeed * targetNearnessFactor;

}

/*---------------------------------------------------------------
					writeToStream
	Implements the writing to a CStream capability of
	  CSerializable objects
  ---------------------------------------------------------------*/
void  CLogFileRecord_VFF::writeToStream(CStream &out,int *version) const
{
	if (version)
		*version = 0;
	else
	{

	}

}

/*---------------------------------------------------------------
					readFromStream
  ---------------------------------------------------------------*/
void  CLogFileRecord_VFF::readFromStream(CStream &in,int version)
{
	switch(version)
	{
	case 0:
		{

		} break;
	default:
		MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

	};


}

