#include "tmachine.h"
#include "tcurves.h"
#include "tcurveutil.h"
#include "tmathutil.h"
#include "tbezier.h"
 
using namespace std;
 
//=============================================================================
 
ostream &operator<<(ostream &out, const TSegment &segment) {
  return out << "S{" << segment.getP0() << ", " << segment.getP1() << "}";
}
 
//=============================================================================
 
void TCubic::split(double t, TCubic &first, TCubic &second) const {
  double s = 1.0 - t;
 
  TPointD H = s * m_p1 + t * m_p2;
 
  first.m_p0 = m_p0;
  first.m_p1 = s * m_p0 + t * m_p1;
  first.m_p2 = s * first.m_p1 + t * H;
 
  second.m_p3 = m_p3;
  second.m_p2 = s * m_p2 + t * m_p3;
  second.m_p1 = s * H + t * second.m_p2;
 
  first.m_p3  = s * first.m_p2 + t * second.m_p1;
  second.m_p0 = first.m_p3;
}
 
double TCubic::getLength(double t0, double t1) const { return -1; }
 
//=============================================================================
 
TPointD TQuadratic::getPoint(double t) const {
  double s = 1 - t;
  return m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2;
}
 
//-----------------------------------------------------------------------------
 
double TQuadratic::getX(double t) const {
  double s = 1 - t;
  return m_p0.x * s * s + 2 * t * s * m_p1.x + t * t * m_p2.x;
}
//-----------------------------------------------------------------------------
 
double TQuadratic::getY(double t) const {
  double s = 1 - t;
  return m_p0.y * s * s + 2 * t * s * m_p1.y + t * t * m_p2.y;
}
//-----------------------------------------------------------------------------
 
double TQuadratic::getT(const TPointD &p) const {
  // risolvo l'equazione  min|| b(t) - p ||
 
  // esprimo b in forma di polinomio  ed ottengo
  //
  //    ||  2                ||
  // min || a t + b t + c - p ||
  //    ||                   ||
  //
  // il tutto si riconduce a cercare le radici
  //  di un'equazione del tipo
  //    2  3          2               2
  // 2·a ·t  + 3·a·b·t  + t·(2·a·v + b ) + b·v
  // dove v e' pari a c - p
 
  vector<TPointD> bez(3), poly(3);
 
  bez[0] = m_p0;
  bez[1] = m_p1;
  bez[2] = m_p2;
 
  bezier2poly(bez, poly);
 
  TPointD v = poly[0] - p;
 
  vector<double> toSolve(4);
  vector<double> sol;
 
  toSolve[3] = 2.0 * norm2(poly[2]);
  toSolve[2] = 3.0 * (poly[2].x * poly[1].x + poly[2].y * poly[1].y);
  toSolve[1] = 2.0 * (poly[2].x * v.x + poly[2].y * v.y) + norm2(poly[1]);
  toSolve[0] = (poly[1].x * v.x + poly[1].y * v.y);
 
  int nSol = rootFinding(toSolve, sol);
 
  if (-1 == nSol)  // infinite soluzioni
    return 0;
 
  int minParameter = -1;
  double minDist   = (std::numeric_limits<double>::max)();
 
  for (int i = 0; i < nSol; ++i) {
    if (sol[i] < 0.0)
      sol[i] = 0.0;
    else if (sol[i] > 1.0)
      sol[i] = 1.0;
 
    double tmpDist = tdistance2(p, getPoint(sol[i]));
    if (tmpDist < minDist) {
      minDist      = tmpDist;
      minParameter = i;
    }
  }
 
  if (minParameter != -1) return sol[minParameter];
 
  return tdistance2(m_p0, p) < tdistance2(m_p2, p) ? 0 : 1;
}
 
//-----------------------------------------------------------------------------
 
void TQuadratic::split(double t, TQuadratic &left, TQuadratic &right) const {
  double dt;
  TPointD p;
  dt = 1.0 - t;
 
  left.m_p0  = m_p0;
  right.m_p2 = m_p2;
 
  left.m_p1  = dt * m_p0 + t * m_p1;
  right.m_p1 = dt * m_p1 + t * m_p2;
  p          = dt * left.m_p1 + t * right.m_p1;
 
  left.m_p2 = right.m_p0 = p;
}
 
//-----------------------------------------------------------------------------
 
TRectD TQuadratic::getBBox() const {
  TRectD bBox;
  if (m_p0.x < m_p2.x)
    bBox.x0 = m_p0.x, bBox.x1 = m_p2.x;
  else
    bBox.x0 = m_p2.x, bBox.x1 = m_p0.x;
 
  if (m_p0.y < m_p2.y)
    bBox.y0 = m_p0.y, bBox.y1 = m_p2.y;
  else
    bBox.y0 = m_p2.y, bBox.y1 = m_p0.y;
 
  TPointD denom = 2 * m_p1 - m_p0 - m_p2;
  if (denom.x != 0) {
    double tx = (m_p1.x - m_p0.x) / denom.x;
    if (tx >= 0 && tx <= 1) {
      double x = getPoint(tx).x;
      if (x < bBox.x0)
        bBox.x0 = x;
      else if (x > bBox.x1)
        bBox.x1 = x;
    }
  }
  if (denom.y != 0) {
    double ty = (m_p1.y - m_p0.y) / denom.y;
    if (ty >= 0 && ty <= 1) {
      double y = getPoint(ty).y;
      if (y < bBox.y0)
        bBox.y0 = y;
      else if (y > bBox.y1)
        bBox.y1 = y;
    }
  }
 
  return bBox;
}
 
/*!
Calcolo della curvatura per una Quadratica.
Vedi Farin pag.176 per la spiegazione della formula
usata.
*/
double TQuadratic::getCurvature(double t) const {
  assert(0 <= t && t <= 1.0);
 
  TQuadratic q1, q2;
 
  split(t, q1, q2);
 
  double signum = 1.0;
  if (areAlmostEqual(t, 1.0)) {
    signum *= -1.0;
    std::swap(q1, q2);
    std::swap(q2.m_p0, q2.m_p2);
  }
 
  TPointD v_1_0(q2.m_p1 - q2.m_p0);
 
  double a = norm2(v_1_0);
 
  if (isAlmostZero(a)) return (std::numeric_limits<double>::max)();
 
  a = 1.0 / sqrt(a);
 
  double b = cross(v_1_0 * a, q2.m_p2 - q2.m_p0);
 
  return 0.5 * signum * b / a;
}
 
//-----------------------------------------------------------------------------
 
double TQuadratic::getLength(double t0, double t1) const {
  TQuadraticLengthEvaluator lengthEval(*this);
 
  t0 = min(max(0.0, t0), 1.0);  // backward compatibility
  t1 = min(max(0.0, t1), 1.0);  // backward compatibility
  if (t0 > t1) std::swap(t0, t1);
 
  if (t0 > 0.0) return lengthEval.getLengthAt(t1) - lengthEval.getLengthAt(t0);
 
  return lengthEval.getLengthAt(t1);
}
 
double TQuadratic::getApproximateLength(double t0, double t1,
                                        double error) const {
  if (t0 == t1) return 0;
 
  t0 = min(max(0.0, t0), 1.0);
  t1 = min(max(0.0, t1), 1.0);
 
  if (t0 > t1) std::swap(t0, t1);
 
  TQuadratic q;
 
  if (t0 == 0.0 && t1 == 1.0)
    q = *this;
  else {
    TQuadratic q1;
    split(t0, q, q1);
 
    assert(t0 != 1.0);
 
    double newPar = (t1 - t0) / (1.0 - t0);
    q1.split(newPar, q, q1);
  }
 
  double step = computeStep(q, error);
 
  double length = 0.0;
 
  TPointD p1 = q.getP0();
  TPointD p2;
  for (double t = step; t < 1.0; t += step) {
    p2 = q.getPoint(t);
    length += tdistance(p1, p2);
    p1 = p2;
  }
  length += tdistance(p1, q.getP2());
 
  return length;
}
 
//-----------------------------------------------------------------------------
 
int TQuadratic::getX(double y, double &x0, double &x1) const {
  int ret = 0;
  double t;
 
  if (y > getBBox().y1 || y < getBBox().y0) return 0;
 
  double a      = getP0().y - 2 * getP1().y + getP2().y;
  double half_b = getP1().y - getP0().y;
  double c      = getP0().y - y;
 
  if (a == 0)  // segment
  {
    if (half_b == 0)  // horizontal segment, or point
    {
      if (c == 0) {
        x0 = getP0().x;
        x1 = getP2().x;
        return 2;
      } else
        return 0;
    } else {
      t = -c / (2 * half_b);
      if (t >= 0 && t <= 1) {
        x0 = getPoint(t).x;
        return 1;
      }
    }
  }
 
  double discr = half_b * half_b - a * c;
 
  if (discr < 0) return 0;
 
  double coeff  = 1.0 / a;
  double coeff1 = -half_b * coeff;
 
  if (discr == 0) {
    t = coeff1;
    if (t >= 0 && t <= 1) {
      ret = 2;
      x0 = x1 = getPoint(t).x;
    }
  } else {
    discr = sqrt(discr) * coeff;
    t     = coeff1 + discr;
    if (t >= 0 && t <= 1) {
      ret++;
      x0 = getPoint(t).x;
    }
 
    t = coeff1 - discr;
    if (t >= 0 && t <= 1) {
      ret++;
      if (ret == 2)
        x1 = getPoint(t).x;
      else
        x0 = getPoint(t).x;
    }
  }
  return ret;
}
 
int TQuadratic::getY(double y, double &y0, double &y1) const {
  TQuadratic temp(*this);
 
  swap(temp.m_p0.x, temp.m_p0.y);
  swap(temp.m_p1.x, temp.m_p1.y);
  swap(temp.m_p2.x, temp.m_p2.y);
 
  return temp.getX(y, y0, y1);
}
//=============================================================================
TPointD TCubic::getPoint(double t) const {
  double s = 1 - t;
  return m_p0 * s * s * s + 3 * t * s * (s * m_p1 + t * m_p2) +
         t * t * t * m_p3;
}
//-----------------------------------------------------------------------------
TPointD TCubic::getSpeed(double t) const {
  double s = 1 - t;
  return 3.0 * ((m_p1 - m_p0) * s * s + 2 * (m_p2 - m_p0) * s * t +
                (m_p3 - m_p2) * t * t);
}
//=============================================================================
 
TThickQuadratic::TThickQuadratic()
    : TQuadratic(), m_thickP0(0), m_thickP1(0), m_thickP2(0) {}
 
//-----------------------------------------------------------------------------
 
TThickQuadratic::TThickQuadratic(const TQuadratic &q)
    : TQuadratic(q), m_thickP0(0.0), m_thickP1(0.0), m_thickP2(0.0) {}
 
//-----------------------------------------------------------------------------
 
TThickQuadratic::TThickQuadratic(const TPointD &p0, double thickP0,
                                 const TPointD &p1, double thickP1,
                                 const TPointD &p2, double thickP2)
    : TQuadratic(p0, p1, p2)
    , m_thickP0(thickP0)
    , m_thickP1(thickP1)
    , m_thickP2(thickP2) {}
 
//-----------------------------------------------------------------------------
 
TThickQuadratic::TThickQuadratic(const TThickPoint &p0, const TThickPoint &p1,
                                 const TThickPoint &p2)
    : TQuadratic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y))
    , m_thickP0(p0.thick)
    , m_thickP1(p1.thick)
    , m_thickP2(p2.thick) {}
 
//-----------------------------------------------------------------------------
 
TThickQuadratic::TThickQuadratic(const TThickQuadratic &thickQuadratic)
    : TQuadratic(thickQuadratic)
    , m_thickP0(thickQuadratic.m_thickP0)
    , m_thickP1(thickQuadratic.m_thickP1)
    , m_thickP2(thickQuadratic.m_thickP2) {}
 
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP0(const TThickPoint &p) {
  m_p0      = p;
  m_thickP0 = p.thick;
}
 
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP1(const TThickPoint &p) {
  m_p1      = p;
  m_thickP1 = p.thick;
}
 
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP2(const TThickPoint &p) {
  m_p2      = p;
  m_thickP2 = p.thick;
}
 
//-----------------------------------------------------------------------------
TThickPoint TThickQuadratic::getThickPoint(double t) const {
  double s = 1 - t;
  return TThickPoint(
      m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2,
      m_thickP0 * s * s + 2 * t * s * m_thickP1 + t * t * m_thickP2);
}
 
//-----------------------------------------------------------------------------
void TThickQuadratic::split(double t, TThickQuadratic &left,
                            TThickQuadratic &right) const {
  double dt;
  TPointD p;
  dt = 1.0 - t;
 
  // control points
  left.m_p0  = m_p0;
  right.m_p2 = m_p2;
 
  left.m_p1  = dt * m_p0 + t * m_p1;
  right.m_p1 = dt * m_p1 + t * m_p2;
  p          = dt * left.m_p1 + t * right.m_p1;
 
  left.m_p2 = right.m_p0 = p;
 
  // thick points
  left.m_thickP0  = m_thickP0;
  right.m_thickP2 = m_thickP2;
 
  left.m_thickP1  = dt * m_thickP0 + t * m_thickP1;
  right.m_thickP1 = dt * m_thickP1 + t * m_thickP2;
 
  // store thickness of intermediary point
  p.x = dt * left.m_thickP1 + t * right.m_thickP1;
 
  left.m_thickP2 = right.m_thickP0 = p.x;
}
 
//-----------------------------------------------------------------------------
 
TRectD TThickQuadratic::getBBox() const {
  TRectD bBox = TQuadratic::getBBox();
 
  double maxRadius = std::max({m_thickP0, m_thickP1, m_thickP2});
  if (maxRadius > 0) {
    //  bBox.enlarge(maxRadius) si comporta male nel caso bBox.isEmpty()
    bBox.x0 -= maxRadius;
    bBox.y0 -= maxRadius;
 
    bBox.x1 += maxRadius;
    bBox.y1 += maxRadius;
  }
 
  return bBox;
}
 
// ============================================================================
// Methods of the class TThickCubic
// ============================================================================
 
TThickCubic::TThickCubic()
    : TCubic(), m_thickP0(0), m_thickP1(0), m_thickP2(0), m_thickP3(0) {}
 
//-----------------------------------------------------------------------------
 
TThickCubic::TThickCubic(const TPointD &p0, double thickP0, const TPointD &p1,
                         double thickP1, const TPointD &p2, double thickP2,
                         const TPointD &p3, double thickP3)
    : TCubic(p0, p1, p2, p3)
    , m_thickP0(thickP0)
    , m_thickP1(thickP1)
    , m_thickP2(thickP2)
    , m_thickP3(thickP3) {}
 
//-----------------------------------------------------------------------------
 
TThickCubic::TThickCubic(const TThickPoint &p0, const TThickPoint &p1,
                         const TThickPoint &p2, const TThickPoint &p3)
    : TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y),
             TPointD(p3.x, p3.y))
    , m_thickP0(p0.thick)
    , m_thickP1(p1.thick)
    , m_thickP2(p2.thick)
    , m_thickP3(p3.thick) {}
//  tonino ***************************************************************
 
TThickCubic::TThickCubic(const T3DPointD &p0, const T3DPointD &p1,
                         const T3DPointD &p2, const T3DPointD &p3)
    : TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y),
             TPointD(p3.x, p3.y))
    , m_thickP0(p0.z)
    , m_thickP1(p1.z)
    , m_thickP2(p2.z)
    , m_thickP3(p3.z) {}
 
//  tonino ***************************************************************
 
//-----------------------------------------------------------------------------
 
TThickCubic::TThickCubic(const TThickCubic &thickCubic)
    : TCubic(thickCubic)
    , m_thickP0(thickCubic.m_thickP0)
    , m_thickP1(thickCubic.m_thickP1)
    , m_thickP2(thickCubic.m_thickP2)
    , m_thickP3(thickCubic.m_thickP3) {}
 
//-----------------------------------------------------------------------------
 
void TThickCubic::setThickP0(const TThickPoint &p) {
  m_p0.x    = p.x;
  m_p0.y    = p.y;
  m_thickP0 = p.thick;
}
 
//-----------------------------------------------------------------------------
 
void TThickCubic::setThickP1(const TThickPoint &p) {
  m_p1.x    = p.x;
  m_p1.y    = p.y;
  m_thickP1 = p.thick;
}
 
//-----------------------------------------------------------------------------
 
void TThickCubic::setThickP2(const TThickPoint &p) {
  m_p2.x    = p.x;
  m_p2.y    = p.y;
  m_thickP2 = p.thick;
}
 
//-----------------------------------------------------------------------------
void TThickCubic::setThickP3(const TThickPoint &p) {
  m_p3.x    = p.x;
  m_p3.y    = p.y;
  m_thickP3 = p.thick;
}
 
//-----------------------------------------------------------------------------
 
TThickPoint TThickCubic::getThickPoint(double t) const {
  double thick_l1, thick_h, thick_r3;
 
  double s = 1.0 - t;
 
  TPointD l1(m_p0 * s + m_p1 * t);
  thick_l1 = m_thickP0 * s + m_thickP1 * t;
 
  TPointD h(m_p1 * s + m_p2 * t);
  thick_h = m_thickP1 * s + m_thickP2 * t;
 
  TPointD r3(m_p2 * s + m_p3 * t);
  thick_r3 = m_thickP2 * s + m_thickP3 * t;
 
  // adesso riutilizzo le variabili gia' utilizzate
 
  // l2
  l1       = l1 * s + h * t;
  thick_l1 = thick_l1 * s + thick_h * t;
 
  // r1
  r3       = h * s + r3 * t;
  thick_r3 = thick_h * s + thick_r3 * t;
 
  // l3-r0
  h       = l1 * s + r3 * t;
  thick_h = thick_l1 * s + thick_r3 * t;
 
  return TThickPoint(h, thick_h);
}
 
//-----------------------------------------------------------------------------
 
void TThickCubic::split(double t, TThickCubic &first,
                        TThickCubic &second) const {
  double s = 1.0 - t;
 
  TPointD H(m_p1 * s + m_p2 * t);
  double thick_h = m_thickP1 * s + m_thickP2 * t;
 
  first.m_p0      = m_p0;
  first.m_thickP0 = m_thickP0;
 
  first.m_p1      = m_p0 * s + m_p1 * t;
  first.m_thickP1 = m_thickP0 * s + m_thickP1 * t;
 
  first.m_p2      = first.m_p1 * s + H * t;
  first.m_thickP2 = first.m_thickP1 * s + thick_h * t;
 
  second.m_p3      = m_p3;
  second.m_thickP3 = m_thickP3;
 
  second.m_p2      = m_p2 * s + m_p3 * t;
  second.m_thickP2 = m_thickP2 * s + m_thickP3 * t;
 
  second.m_p1      = H * s + second.m_p2 * t;
  second.m_thickP1 = thick_h * s + second.m_thickP2 * t;
 
  first.m_p3      = first.m_p2 * s + second.m_p1 * t;
  first.m_thickP3 = first.m_thickP2 * s + second.m_thickP1 * t;
 
  second.m_p0      = first.m_p3;
  second.m_thickP0 = first.m_thickP3;
}
 
//-----------------------------------------------------------------------------
 
ostream &operator<<(ostream &out, const TQuadratic &curve) {
  return out << "Q{" << curve.getP0() << ", " << curve.getP1() << ", "
             << curve.getP2() << "}";
}
 
ostream &operator<<(ostream &out, const TCubic &curve) {
  return out << "C{" << curve.getP0() << ", " << curve.getP1() << ", "
             << curve.getP2() << ", " << curve.getP3() << "}";
}
 
ostream &operator<<(ostream &out, const TThickSegment &segment) {
  return out << "TS{" << segment.getThickP0() << ", " << segment.getThickP1()
             << "}";
}
 
ostream &operator<<(ostream &out, const TThickQuadratic &tq) {
  return out << "TQ{" << tq.getThickP0() << ", " << tq.getThickP1() << ", "
             << tq.getThickP2() << "}";
}
 
ostream &operator<<(ostream &out, const TThickCubic &tc) {
  return out << "TC{" << tc.getThickP0() << ", " << tc.getThickP1() << ", "
             << tc.getThickP2() << ", " << tc.getThickP3() << "}";
}
 
//-----------------------------------------------------------------------------
//  End Of File
//-----------------------------------------------------------------------------