/* +---------------------------------------------------------------------------+
   |          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/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#include "CPTG2.h"

using namespace MRPTAPPS::RNAV;

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CPTG2::CPTG2(
		float	refDistance,
		float	xResolution,
		float	yResolution,
		float	V_MAX,
		float	W_MAX,
		float	system_TAU,
		float	system_DELAY,
		vector_float	securityDistances,
		float	cte_a0v,
		float	cte_a0w
		)
					: CParameterizedTrajectoryGenerator( refDistance,xResolution,yResolution,V_MAX,W_MAX,system_TAU,system_DELAY,securityDistances  )

{
	this->cte_a0v=cte_a0v;
	this->cte_a0w=cte_a0w;
}

/*---------------------------------------------------------------
						getDescription
  ---------------------------------------------------------------*/
std::string CPTG2::getDescription()
{
	char str[100];
	MRPT_OS::sprintf(str,100,"Type#2PTG,av=%udeg,aw=%udeg",(int)RAD2DEG(cte_a0v),(int)RAD2DEG(cte_a0w) );
	return std::string(str);
}


/*---------------------------------------------------------------
						PTG_Generator
  ---------------------------------------------------------------*/
void CPTG2::PTG_Generator( float alfa, float t,float x, float y, float phi, float &v, float &w )
{
    float At_a = alfa - phi;

    while (At_a>M_PI) At_a -= (float) M_2PI;
    while (At_a<-M_PI) At_a += (float) M_2PI;

    v = V_MAX * exp(-square( At_a / cte_a0v ));
    w=  W_MAX * (-0.5f + (1/(1+exp(-At_a/cte_a0w))));
}

/*---------------------------------------------------------------
					PTG_IsIntoDomain
  ---------------------------------------------------------------*/
bool CPTG2::PTG_IsIntoDomain( float x, float y )
{
	return true;
}

/*---------------------------------------------------------------
					lambdaFunction
  ---------------------------------------------------------------*/
void CPTG2::lambdaFunction( float x, float y, int &out_k, float &out_d )
{
	CParameterizedTrajectoryGenerator::lambdaFunction(x,y,out_k,out_d);
}
