#include <cassert>

#include <mathlib.h>
#include <d2homogeneous.h>

#include <d2arrow.h>




double d2arrow::headlengthfactor = 0.8;

void d2arrow::translate( point2<double> const & x )
{
  for (uint i=0; i<4; ++i)
    pi[i] += x; 
}

void d2arrow::rotate(doublec theta)
{
  d2homogeneous R;

  R.setRotateAboutPoint(theta,pi[0]);

  for (uint i=1; i<4; ++i)
    R.matrixMultiply(pi[i]);
}

d2arrow::d2arrow
( 
  doublec arrowlength, 
  doublec headlength 
)
{
  constructOnXaxis(arrowlength,headlength);
}

d2arrow::d2arrow
(
  point2<double> const & pi0,
  doublec theta, 
  doublec arrowlength, 
  doublec headlength
)
{
  constructOnXaxis(arrowlength,headlength);
  translate(pi0);
  rotate(theta);
}

d2arrow::d2arrow
(
  point2<double> const & q0,
  point2<double> const & q1,
  doublec arrowlength, 
  doublec headlength
)
{
  // Using vector calculus to calculate the arrows
  // geometry points.

  // Construct unit vectors z1 and z2 where z2 is the normal.
  // Further z1 = z2 rotated 90 degrees anticlockwise direction.

  // z1 and z2 are at right angles and form a coordinate system.

  point2<double> z1(q1-q0);
  z1.normalize();
  point2<double> z2(-z1.y,z1.x);

  // The head base of the arrow.
  point2<double> hb(z2);
  hb *= (arrowlength-headlength);

  // Half the hight of the arrow head.
  double w = headlength*headlengthfactor*0.5;

  pi[2] = hb - z1 * w;
  pi[3] = hb + z1 * w;
  pi[1] = z2*arrowlength;
  pi[0] = point2<double>(0.0,0.0);

  point2<double> p0(q0+q1);
  p0 *= 0.5;
  translate(p0);
}


void d2arrow::constructOnXaxis
( 
  doublec arrowlength, 
  doublec headlength 
)
{
  assert(arrowlength>headlength);
  assert(arrowlength>0.0);

  pi[0] = point2<double>(0.0,0.0);
  pi[1] = point2<double>(arrowlength,0.0);
  doublec q = arrowlength-headlength;
  doublec arrowheight = headlength*headlengthfactor*0.5;
  pi[2] = point2<double>(q,arrowheight);
  pi[3] = point2<double>(q,-arrowheight);
}

void d2arrow::draw()
{
  glBegin(GL_LINES);

  glVertex3f(pi[0].x,pi[0].y,0.0);
  glVertex3f(pi[1].x,pi[1].y,0.0);

  glEnd();

  glBegin(GL_TRIANGLES);

  glVertex3f(pi[1].x,pi[1].y,0.0);
  glVertex3f(pi[2].x,pi[2].y,0.0);
  glVertex3f(pi[3].x,pi[3].y,0.0);

  glEnd();
}