#include <cassert>
#include <sstream>
using namespace std;


#include <disk.h>
#include <mathlib.h>
#include <zero.h>

disk::disk
(
  point3<double> const & nml_,
  point3<double> const & pos_,
  doublec radius_
)
  : nml(nml_), pos(pos_), radius(radius_)
{
//cout << SHOW(nml.dot()) << endl;
//  assert(nml.dot()<=1.0+zero<double>::val);
}


boolc disk::intersects
(
  point3<double> & p0,
  point3<double> & p1,
  disk const & D2
) const
{
  // Four intersection points of disks on line.
  point3<double> q[4];

  plane w2(D2.nml,D2.nml.dot(D2.pos));
  if (intersects(q[0],q[1],w2)==false)
    return false;

  plane w1(nml,nml.dot(pos));
  if (D2.intersects(q[2],q[3],w1)==false)
    return false;

  // Given line segments [q0,q1] and [q3,q4],
  // determine if they overlap and the start and end points.
  // These lines should be colinear.
  // <TODO>

  
  point3<double> linegrad(q[2]-q[0]);
  if (zero<double>::test(linegrad.dot()))
  {
    linegrad = q[3]-q[0];
    // The disks are touching.
    if (zero<double>::test(linegrad.dot()))
    {
      p0 = q[0];
      p1 = q[0];
      return true;
    }
  };

  // Let the line by q[0] + linegrad*t.

  double t[4];
  t[0] = 0.0;

  if (zero<double>::test(linegrad.x*linegrad.x))
  {
    assert(zero<double>::test(linegrad.y*linegrad.y)==false);

    for (uint i=1; i<4; ++i)
      t[i] = (q[i].y-q[0].y) / linegrad.y;
  }  
  else
  {
    for (uint i=1; i<4; ++i)
      t[i] = (q[i].x-q[0].x) / linegrad.x;
  }

#ifndef NDEBUG
  for (uint i=0; i<4; ++i)
  {
    //cout << SHOW( (q[i]-(q[0]+linegrad*t[i])).dot() ) << endl;
    assert( zero<double>::test((q[i]-(q[0]+linegrad*t[i])).dot()) );
  }
#endif

  double c0;
  double c1;
  if (intervalintersection::unordered(c0,c1,t[0],t[1],t[2],t[3])==false)
    return false;

  p0 = q[0] + linegrad*c0;
  p1 = q[0] + linegrad*c1;

  return true;
}


boolc disk::intersects
(
  point3<double> & p0,
  point3<double> & p1,
  plane const & w2
) const
{
  plane w1(nml,nml.dot(pos));

  // linept + linegrad*t
  point3<double> linept;
  point3<double> linegrad;

  bool res = w1.intersects(linept,linegrad,w2); 
  if (res==false)
    return false;

  // Make a coordinate system relative to the disks origin.
  point3<double> q0(linept-pos);
  point3<double> q1(linept+linegrad-pos);

  // armx and army are the arms of the 2D coordinate system
  // in 3D space.
  point3<double> armx(q0);
  if (zero<double>::test(armx.dot()))
  {
    //t=1 case
    armx += linegrad;
    if (zero<double>::test(armx.dot()))
      return false;

    q0=linept+linegrad-pos;
    q1=linept-pos;
  }
  armx.normalize();

  point3<double> army;
  crossproduct::evalxyz(army,armx,nml);
  assert(zero<double>::test(army.dot())==false);

  // Get the line points in the 2D coordinate system representation.
  point2<double> Q0(armx.dot(q0),0.0);
  assert(zero<double>::test(army.dot(q0)));
  point2<double> Q1(armx.dot(q1),army.dot(q1));

  point2<double> P0;
  point2<double> P1;
  res = circleLine::intersection2D(P0,P1,radius,Q0,Q1);
  if (res==false)
    return false;

  p0 = armx*P0.x + army*P0.y + pos;
  p1 = armx*P1.x + army*P1.y + pos;

  return true;
}

disk::operator stringc () const
{
  stringstream ss;
  ss << nml.x << " " << nml.y << " " << nml.z << "  ";
  ss << pos.x << " " << pos.y << " " << pos.z << "  "; 
  ss << radius;

  return ss.str();
}