/*
 * This file is part of the Owl Positioning System (OwlPS) project.
 * It is subject to the copyright notice and license terms in the
 * COPYRIGHT.t2t file found in the top-level directory of this
 * distribution and at
 *   http://code.lm7.fr/p/owlps/source/tree/master/COPYRIGHT.t2t
 * No part of the OwlPS Project, including this file, may be copied,
 * modified, propagated, or distributed except according to the terms
 * contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
 * distributed along with this file, either separately or by replacing
 * this notice by the COPYRIGHT.t2t file's contents.
 */


#include "point3d.hh"
#include "posutil.hh"
#include "posexcept.hh"

#include <sstream>

// For interpolate()
#include <claw/tween/tweener_group.hpp>
#include <claw/tween/single_tweener.hpp>
#include <claw/tween/easing/easing_linear.hpp>

using namespace std ;



/* *** Constructors *** */


Point3D::Point3D(const string &source)
{
  float pos[3] ;
  istringstream iss(source) ;

  for (int i = 0 ; i < 2 ; ++i)
    {
      iss >> pos[i] ;
      if (iss.get() != ';')
        throw malformed_input_data(
          "Point3D(string): cannot extract coordinates!") ;
    }
  iss >> pos[2] ;

  set_coordinates(pos) ;
}



/* *** Distance operations *** */


/**
 * The distance is not square rooted after the computation, in order
 * to optimise comparisons.
 */
float Point3D::square_distance_2d(const Point3D &source) const
{
  return
    (x - source.x) * (x - source.x) +
    (y - source.y) * (y - source.y) ;
}


/**
 * The distance is not square rooted after the computation, in order
 * to optimise comparisons.
 */
float Point3D::square_distance(const Point3D &source) const
{
  return
    (x - source.x) * (x - source.x) +
    (y - source.y) * (y - source.y) +
    (z - source.z) * (z - source.z) ;
}


/**
 * A, B and C are three points, A being the current Point3D (*this).
 * This function computes the angle BÂC, in two dimensions (i.e. the Z
 * coordinate of the points is ignored).
 *
 * If the points are aligned, the angle returned is always 0° (and not
 * 180°, even in the case where A is on [BC]).
 *
 * @returns The angle BÂC, in the interval [0, 180[ degrees.
 */
double Point3D::angle_2d(const Point3D &b, const Point3D &c) const
{
  double
  sq_ab = square_distance_2d(b),
  sq_ac = square_distance_2d(c),
  sq_bc = b.square_distance_2d(c) ;

  if (sq_ab == 0 || sq_ac == 0 || sq_bc == 0)
    return 0 ;

  double
  ab = sqrt(sq_ab),
  ac = sqrt(sq_ac) ;

  double cos_bac = (sq_ab + sq_ac - sq_bc) / (2 * ab * ac) ;
  double bac = acos(cos_bac) ;

  return PosUtil::rad2deg(bac) ;
}


/**
 * #x and #y are updated with the rotated image of the point.
 *
 * Note that as this functions does a 2D rotation, the c.z is ignored,
 * and so is this->x.
 */
void Point3D::rotate_2d(const Point3D &c, float angle)
{
  angle = PosUtil::deg2rad(angle) ;

  float new_x = cos(angle) * (x-c.x) - sin(angle) * (y-c.y) + c.x ;
  float new_y = sin(angle) * (x-c.x) + cos(angle) * (y-c.y) + c.y ;

  x = new_x ;
  y = new_y ;
}



/* *** Other operations *** */


/**
 * This function computes the intermediate (linear) values between the
 * current point and the point `end`.
 *
 * The number of steps is function of `step_hint` and the distance
 * between the two points. `step_hint`, as its name indicates, is a hint
 * of the desired distance between two intermediate coordinates. It is
 * adjusted by the function to distribute equally the intermediate
 * coordinates between the two extremities.
 *
 * @param[in] end Coordinate of the last extremity.
 * @param[in,out] step_hint The desired approximate distance between two
 * intermediate points; when the function returns, it is set to the
 * actual step used.
 * @param[out] interpolated The vector in which the intermediate values
 * will be stored. The two extremities are not included in this vector,
 * but of course the calling function can add values prior and after the
 * function call.
 */
void Point3D::interpolate(const Point3D &end, float &step_hint,
                          std::vector<Point3D> &interpolated) const
{
  /**
   * Example of step computation: if the distance between the two
   * extremities (`*this` and `end`) is 12.42 m and `step_hint` is set
   * to 2 m, the number of steps is 12 / 2 == 6, and the actual distance
   * between two intermediate coordinates is 12.42 / 6 == 2.07 m.
   */
  float dist = distance(end) ;
  int nb_steps = dist / step_hint ;
  step_hint = dist / nb_steps ;

  /* Set up the tweener */
  // Individual tweeners for X, Y and Z
  double tweened_x = x, tweened_y = y, tweened_z = z ;
  claw::tween::single_tweener
  interpolator_x(tweened_x, end.x, dist,
                 claw::tween::easing_linear::ease_in) ;
  claw::tween::single_tweener
  interpolator_y(tweened_y, end.y, dist,
                 claw::tween::easing_linear::ease_in) ;
  claw::tween::single_tweener
  interpolator_z(tweened_z, end.z, dist,
                 claw::tween::easing_linear::ease_in) ;
  // Consolidated tweener for both X and Y
  claw::tween::tweener_group interpolator ;
  interpolator.insert(interpolator_x) ;
  interpolator.insert(interpolator_y) ;
  interpolator.insert(interpolator_z) ;

  /* Compute interpolated values */
  do
    {
      // Update tweened_x, tweened_y, tweened_z
      interpolator.update(step_hint);
      // Add the new coordinates to the vector
      interpolated.push_back(Point3D(tweened_x, tweened_y, tweened_z)) ;
    }
  while (! interpolator.is_finished()) ;
}



/* *** Operators *** */


Point3D& Point3D::operator=(const Point3D &source)
{
  if (this == &source)
    return *this ;

  x = source.x ;
  y = source.y ;
  z = source.z ;

  return *this ;
}


bool Point3D::operator==(const Point3D &source) const
{
  if (this == &source)
    return true ;

  return
    x == source.x &&
    y == source.y &&
    z == source.z ;
}


bool Point3D::operator<(const Point3D &source) const
{
  if (x < source.x)
    return true ;
  if (x > source.x)
    return false ;

  if (y < source.y)
    return true ;
  if (y > source.y)
    return false ;

  if (z < source.z)
    return true ;

  return false ;
}


Point3D::operator std::string(void) const
{
  ostringstream oss ;
  oss << *this ;
  return oss.str() ;
}


/**
 * @returns `true` if either #x, #y or #z is non-zero.
 * @returns `false` if #x, #y and #z are defined to 0.
 */
Point3D::operator bool(void) const
{
  return
    x != 0 ||
    y != 0 ||
    z != 0 ;
}



ostream& operator<<(ostream &os, const Point3D &p)
{
  os << "(" << p.x << ";" << p.y << ";" << p.z << ")" ;
  return os ;
}