#include <tools assistants="" guidelineellipse.h=""></tools>

// TnzCore includes

#include <tgl.h></tgl.h>

#include <tmathutil.h></tmathutil.h>

// returns a point nearest to the ellipse with center in zero

static TPointD findNearestPoint(const TPointD &p, double Rx, double Ry) {

  Rx = fabs(Rx);

  Ry = fabs(Ry);

  TPointD ep;

  if (isAlmostZero(Rx)) {

    ep.y = p.y < -Ry ? -Ry

         : p.y >  Ry ?  Ry : p.y;

    return ep;

  }

  if (isAlmostZero(Ry)) {

    ep.x = p.x < -Rx ? -Rx

         : p.x >  Rx ?  Rx : p.x;

    return ep;

  }

  double k = 1/Rx;

  double x0 = p.x*k;

  double y0 = p.y*k;

  k *= Ry;

  k *= k; // k = (Ry/Rx)^2

  double l = k - 1;

  double a = l*l;

  double b = 2*l*x0;

  double c = x0*x0 + y0*y0*k - l*l;

  double d = -b;

  double e = -x0*x0;

  double dist = INFINITY;

  std::complex<double> roots[4];</double>

  int cnt = solveEquation4(roots, a, b, c, d, e);

  for(int i = 0; i < cnt; ++i) {

    if (!isAlmostZero(roots[i].imag()))

      continue;

    double x = roots[i].real();

    double y;

    if (isAlmostZero(fabs(x) - 1)) {

      y = 0;

    } else

    if (fabs(x) < 1) {

      y = sqrt(k*(1 - x*x));

      if (y0 < 0) y = -y;

    } else {

      continue;

    }

    double dd = (x0-x)*(x0-x) + (y0-y)*(y0-y);

    if (dd < dist)

      { ep.x = x*Rx; ep.y = y*Rx; dist = dd; }

  }

  return ep;

}

//*****************************************************************************************

//    TGuidelineEllipse implementation

//*****************************************************************************************

TGuidelineEllipse::TGuidelineEllipse(

  bool enabled,

  double magnetism,

  const TAffine &matrix,

  const TAffine &matrixInv,

  double Rx,

  double Ry

):

  TGuideline(enabled, magnetism),

  matrix(matrix),

  matrixInv(matrixInv),

  Rx(Rx),

  Ry(Ry)

  { }

TGuidelineEllipse::TGuidelineEllipse(

  bool enabled,

  double magnetism,

  const TAffine &matrix,

  const TAffine &matrixInv

):

  TGuidelineEllipse(

    enabled, magnetism, matrix, matrixInv,

    sqrt(matrix.a11*matrix.a11 + matrix.a21*matrix.a21),

    sqrt(matrix.a12*matrix.a12 + matrix.a22*matrix.a22) )

  { }

TGuidelineEllipse::TGuidelineEllipse(

  bool enabled,

  double magnetism,

  const TAffine &matrix

):

  TGuidelineEllipse(enabled, magnetism, matrix, matrix.inv())

  { }

TTrackPoint

TGuidelineEllipse::transformPoint(const TTrackPoint &point) const

{

  TTrackPoint p = point;

  TPointD pp = matrixInv*p.position;

  double l2 = norm2(pp);

  if (l2 > TConsts::epsilon*TConsts::epsilon)

    p.position = matrix*(pp*(1.0/sqrt(l2)));

  return p;

}

TPointD

TGuidelineEllipse::nearestPoint(const TPointD &point) const

{

  TPointD p = matrixInv*point;

  p = findNearestPoint(TPointD(p.x*Rx, p.y*Ry), Rx, Ry);

  if (!isAlmostZero(Rx)) p.x /= Rx;

  if (!isAlmostZero(Ry)) p.y /= Ry;

  return matrix*p;

}

bool

TGuidelineEllipse::truncateEllipse(

  TAngleRangeSet &ranges,

  const TAffine &ellipseMatrixInv,

  const TRectD &bounds )

{

  if (ranges.isEmpty()) return false;

  if (bounds.isEmpty()) { ranges.clear(); return false; }

  TPointD o  = ellipseMatrixInv*bounds.getP00();

  TPointD dx = ellipseMatrixInv.transformDirection(TPointD(bounds.getLx(), 0.0));

  TPointD dy = ellipseMatrixInv.transformDirection(TPointD(0.0, bounds.getLy()));

  double lx2 = norm2(dx);

  double ly2 = norm2(dy);

  if ( lx2 < TConsts::epsilon*TConsts::epsilon

    || ly2 < TConsts::epsilon*TConsts::epsilon )

      { ranges.clear(); return false; }

  TPointD nx = rotate90(dx)*(1.0/sqrt(lx2));

  TPointD ny = rotate90(dy)*(1.0/sqrt(ly2));

  TAngleI ax = TAngleRangeSet::fromDouble(atan(dx));

  TAngleI ay = TAngleRangeSet::fromDouble(atan(dy));

  double sign = nx*dy;

  if (fabs(sign) <= TConsts::epsilon)

    { ranges.clear(); return false; }

  if (sign < 0.0) {

    nx = -nx; ny = -ny;

    ax ^= TAngleRangeSet::half;

    ay ^= TAngleRangeSet::half;

  }

  TAngleI angles[] = {

    ax,

    ax^TAngleRangeSet::half,

    ay,

    ay^TAngleRangeSet::half };

  double heights[] = {

    o*nx,

    -((o+dx+dy)*nx),

    (o+dx)*ny,

    -((o+dy)*ny) };

  for(int i = 0; i < 4; ++i) {

    double h = heights[i];

    if (heights[i] <= TConsts::epsilon - 1.0)

      continue;

    if (h >= 1.0 - TConsts::epsilon)

      { ranges.clear(); return false; }

    TAngleI a = TAngleRangeSet::fromDouble(asin(h));

    TAngleI da = angles[i];

    ranges.subtract(da - a, (da + a)^TAngleRangeSet::half );

    if (ranges.isEmpty()) return false;

  }

  return true;

}

int

TGuidelineEllipse::calcSegmentsCount(const TAffine &ellipseMatrix, double pixelSize) {

  const TAffine &em = ellipseMatrix;

  const int min = 4, max = 1000;

  double r = sqrt(0.5*(norm2(TPointD(em.a11, em.a21)) + norm2(TPointD(em.a12, em.a22))));

  double h = 0.5*pixelSize/r;

  if (h <= TConsts::epsilon) return max;

  if (h >= 1.0 - TConsts::epsilon) return min;

  double segments = round(M_2PI/acos(1.0 - h));

  return segments <= (double)min ? min

       : segments >= (double)max ? max

       : (int)segments;

}

void

TGuidelineEllipse::draw(bool active, bool enabled) const {

  TAffine4 modelview, projection;

  glGetDoublev(GL_MODELVIEW_MATRIX, modelview.a);

  glGetDoublev(GL_PROJECTION_MATRIX, projection.a);

  TAffine screenMatrix = (projection*modelview).get2d();

  TAffine screenMatrixInv = screenMatrix.inv();

  double pixelSize = sqrt(tglGetPixelSize2());

  const TRectD oneBox(-1.0, -1.0, 1.0, 1.0);

  TAngleRangeSet ranges(true);

  if (!truncateEllipse(ranges, matrixInv*screenMatrixInv, oneBox))

      return;

  int segments = calcSegmentsCount(matrix, pixelSize);

  double da = M_2PI/segments;

  double s = sin(da);

  double c = cos(da);

  for(TAngleRangeSet::Iterator i(ranges); i; ++i) {

    double a0 = i.d0();

    double a1 = i.d1greater();

    TPointD r(cos(a0), sin(a0));

    TPointD p0 = matrix*r;

    int cnt = (int)floor((a1 - a0)/da);

    for(int j = 0; j < cnt; ++j) {

      r = TPointD(r.x*c - r.y*s, r.y*c + r.x*s);

      TPointD p1 = matrix*r;

      drawSegment(p0, p1, pixelSize, active, enabled);

      p0 = p1;

    }

    drawSegment(p0, matrix*TPointD(cos(a1), sin(a1)), pixelSize, active, enabled);

  }

}