#pragma once

#include <QObject>
#include <QPointF>

#include <cmath>

#include "boost/units/systems/angle/degrees.hpp"
#include "boost/units/systems/si.hpp"

template <int k, class Point> auto get(const Point &p);

namespace bu = boost::units;

namespace qty {
using Length = bu::quantity<bu::si::length>;
using Angle = bu::quantity<bu::si::plane_angle>;
using Degree = bu::quantity<bu::degree::plane_angle>;
using Radian = bu::quantity<bu::si::plane_angle>;
} // namespace qty

template <class NumberType, class Point> class GenericCircle {
public:
  GenericCircle();
  GenericCircle(NumberType rad, Point ori);

  // Property setters
  void setRadius(NumberType rad);
  void setOrigin(const Point &ori);

  // Property getters
  NumberType radius() const;
  const Point &origin() const;
  Point &origin();

private:
  NumberType _r;
  Point _origin;
};

// Free functions.
template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
                 Container &c);
template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c);

// Impl.
template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle() : _r(0), _origin(0, 0) {}

template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle(NumberType rad, Point ori)
    : _r(rad), _origin(ori) {}

/*!
 * Returns a polygon with \a n corners which approximates the
 * circle.
 *
 * \sa Point
 */

template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setRadius(NumberType rad) {
  if (rad >= 0) {
    this->_r = rad;
  }
}

template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setOrigin(const Point &ori) {
  this->_origin = ori;
}

template <class NumberType, class Point>
NumberType GenericCircle<NumberType, Point>::radius() const {
  return this->_r;
}

template <class NumberType, class Point>
const Point &GenericCircle<NumberType, Point>::origin() const {
  return this->_origin;
}

template <class NumberType, class Point>
Point &GenericCircle<NumberType, Point>::origin() {
  return this->_origin;
}

template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
                 Container &c) {

  auto clipp = [](double angle) {
    while (angle < 0) {
      angle += 2 * M_PI;
    }
    while (angle > 2 * M_PI) {
      angle -= 2 * M_PI;
    }
    return angle;
  };

  double a1 = clipp(alpha1.value());
  double a2 = clipp(alpha2.value());
  double angleDisc = 0;
  if (n > 0) {
    angleDisc = (a2 - a1) / (n - 1);
  } else {
    angleDisc = (a2 - a1) / (n + 1);
    n = n * (-1);
  }

  double a = a1;
  using Point =
      std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
  auto x0 = get<0>(circ.origin());
  auto y0 = get<1>(circ.origin());
  auto r = circ.radius();
  using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
  while (n--) {
    auto x = NumberType(x0 + r * std::cos(a));
    auto y = NumberType(y0 + r * std::sin(a));
    c.push_back(Point(x, y));
    a += angleDisc;
  }
}

template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c) {
  using namespace bu;

  double angleDisc = 0;
  if (n > 0) {
    angleDisc = 2 * M_PI / (n - 1);
  } else {
    angleDisc = 2 * M_PI / (n + 1);
    n = n * (-1);
  }

  double a = 0;
  using Point =
      std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
  auto x0 = get<0>(circ.origin());
  auto y0 = get<1>(circ.origin());
  auto r = circ.radius();
  using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
  while (n--) {
    auto x = NumberType(x0 + r * std::cos(a));
    auto y = NumberType(y0 + r * std::sin(a));
    c.push_back(Point(x, y));
    a += angleDisc;
  }
}