#include <tools/assistants/guidelineellipse.h>

// TnzCore includes

#include "tgl.h"

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

//    TGuidelineEllipse implementation

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

TGuidelineEllipse::TGuidelineEllipse(

  bool enabled,

  double magnetism,

  TAffine matrix

):

  TGuideline(enabled, magnetism),

  matrix(matrix),

  matrixInv(matrix.inv()) { }

TGuidelineEllipse::TGuidelineEllipse(

  bool enabled,

  double magnetism,

  TAffine matrix,

  TAffine matrixInv

):

  TGuideline(enabled, magnetism),

  matrix(matrix),

  matrixInv(matrixInv) { }

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;

}

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);

  }

}