/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
 *
 * This software 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 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Generic feature (used to create new feature not implemented in ViSP).
 */

/*!
 * \file vpGenericFeature.h
 * \brief class that defines what is a generic feature (used to create new
 * feature not implemented in ViSP2
 */

#ifndef vpGenericFeature_hh
#define vpGenericFeature_hh

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRGBa.h>
#include <visp3/visual_features/vpBasicFeature.h>

#include <math.h>

BEGIN_VISP_NAMESPACE
/*!
 * \class vpGenericFeature
 * \ingroup group_visual_features
 *
 * \brief Class that enables to define a feature or a set of features which are
 * not implemented in ViSP as a specific class. It is indeed possible to create
 * its own features, to use the corresponding interaction matrix, and to compute
 * an error between the current and the desired feature. Moreover the created
 * features can be mixed with features already implemented.
 *
 * The following example shows how to use the vpGenericFeature class to create
 * and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point
 * whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The
 * interaction matrix corresponding to this feature is \f[ L =
 * \left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f].
 * \code
 * #include <visp3/core/vpGenericFeature.h>
 * #include <visp3/vs/vpServo.h>
 *
 * #ifdef ENABLE_VISP_NAMESPACE
 * using namespace VISP_NAMESPACE_NAME;
 * #endif
 *
 * int main()
 * {
 *   vpServo task; // Visual servoing task
 *
 *   //First we have to define the desired feature log(Z*) corresponding to the desired point.
 *   double xd = 0; //The x coordinate of the desired point.
 *   double yd = 0; //The y coordinate of the desired point.
 *   double Zd = 1; //The depth of the desired point.
 *   vpGenericFeature logZd(1); //The dimension of the feature is 1.
 *   logZd.set_s( log(Zd) );
 *
 *   //Then we have to define the current feature log(Z) corresponding to the current point.
 *   double x = 1; //The x coordinate of the current point.
 *   double y = 1; //The y coordinate of the current point.
 *   double Z = 2; //The depth of the current point.
 *   vpGenericFeature logZ(1); //The dimension of the feature is 1.
 *   logZ.set_s( log(Z) );
 *
 *   // Set eye-in-hand control law.
 *   // The computed velocities will be expressed in the camera frame
 *   task.setServo(vpServo::EYEINHAND_CAMERA);
 *   // Interaction matrix is computed with the current visual features sd
 *   task.setInteractionMatrixType(vpServo::CURRENT);
 *
 *   // Add the point feature to the task
 *   task.addFeature(logZ, logZd);
 *
 *   // Control loop
 *   for ( ; ; ) {
 *     // The new parameters x, y and Z must be computed here.
 *
 *     // Update the current point visual feature
 *     logZ.set_s( log(Z) ) ;
 *
 *     // We have to compute the interaction matrix corresponding to the feature.
 *     vpMatrix LlogZ(1,6) ;
 *     LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
 *     LlogZ[0][2] = -1/Z;
 *     LlogZ[0][3] = -y;
 *     LlogZ[0][4] =  x;
 *     logZ.setInteractionMatrix(LlogZ) ;
 *
 *     // compute the control law
 *     vpColVector v = task.computeControlLaw(); // camera velocity
 *   }
 *   return 0;
 * }
 * \endcode
 *
 * The second example shows how to create and use a feature whose specificity is
 * to have a desired feature fixed to zero. It is the case for the feature \f$
 * log( \frac{Z}{Z^*}) \f$.
 *
 * \code
 * #include <visp3/core/vpGenericFeature.h>
 * #include <visp3/vs/vpServo.h>
 *
 * #ifdef ENABLE_VISP_NAMESPACE
 * using namespace VISP_NAMESPACE_NAME;
 * #endif
 *
 * int main()
 * {
 *   vpServo task; // Visual servoing task
 *
 *   //First we have to define the desired feature log(Z*) corresponding to the desired point.
 *   double xd = 0; //The x coordinate of the desired point.
 *   double yd = 0; //The y coordinate of the desired point.
 *   double Zd = 1; //The depth of the desired point.
 *
 *   //Then we have to define the current feature log(Z) corresponding to the current point.
 *   double x = 1; //The x coordinate of the current point.
 *   double y = 1; //The y coordinate of the current point.
 *   double Z = 2; //The depth of the current point.
 *   vpGenericFeature logZ(1); //The dimension of the feature is 1.
 *   logZ.set_s( log(Z/Zd) );
 *
 *   // Set eye-in-hand control law.
 *   // The computed velocities will be expressed in the camera frame
 *   task.setServo(vpServo::EYEINHAND_CAMERA);
 *   // Interaction matrix is computed with the current visual features sd
 *   task.setInteractionMatrixType(vpServo::CURRENT);
 *
 *   // Add the point feature to the task
 *   task.addFeature(logZ);
 *
 *   // Control loop
 *   for ( ; ; ) {
 *     // The new parameters x, y and Z must be computed here.
 *
 *     // Update the current point visual feature
 *     logZ.set_s( log(Z/Zd) ) ;
 *
 *     // We have to compute the interaction matrix corresponding to the feature.
 *     vpMatrix LlogZ(1,6) ;
 *     LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
 *     LlogZ[0][2] = -1/Z;
 *     LlogZ[0][3] = -y;
 *     LlogZ[0][4] =  x;
 *     logZ.setInteractionMatrix(LlogZ) ;
 *
 *     // compute the control law
 *     vpColVector v = task.computeControlLaw(); // camera velocity
 *   }
 *   return 0;
 * }
 * \endcode
 *
 * If the feature needs to be use with other features, the example
 * servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it.
*/
class VISP_EXPORT vpGenericFeature : public vpBasicFeature
{
private:
  vpGenericFeature();

public:
  VP_EXPLICIT vpGenericFeature(unsigned int dim);

  void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
               unsigned int thickness = 1) const VP_OVERRIDE;
  void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
               unsigned int thickness = 1) const VP_OVERRIDE;
  vpGenericFeature *duplicate() const VP_OVERRIDE;
  vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL) VP_OVERRIDE;

  vpColVector error(unsigned int select = FEATURE_ALL);

  vpMatrix getInteractionMatrix() const { return L; }
  void get_s(vpColVector &s) const;
  void get_s(double &s0) const;
  void get_s(double &s0, double &s1) const;
  void get_s(double &s0, double &s1, double &s2) const;

  void init() VP_OVERRIDE;
  vpMatrix interaction(unsigned int select = FEATURE_ALL) VP_OVERRIDE;
  void print(unsigned int select = FEATURE_ALL) const VP_OVERRIDE;
  void setInteractionMatrix(const vpMatrix &L);
  void setError(const vpColVector &error_vector);
  void set_s(const vpColVector &s);
  void set_s(const double s0);
  void set_s(const double s0, const double s1);
  void set_s(const double s0, const double s1, const double s2);

private:
  typedef enum { errorNotInitialized, errorInitialized, errorHasToBeUpdated } vpGenericFeatureErrorType;

  vpMatrix L;
  vpColVector err;
  vpGenericFeatureErrorType errorStatus;
};
END_VISP_NAMESPACE
#endif
