//  $Id$
// 
//   Copyright (C) 2003-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved  @@
//
#include <RDGeneral/Invariant.h>
#include "Transform.h"
#include "Transform2D.h"
#include <math.h>
#include "point.h"

namespace RDGeom {
  
  void Transform2D::setToIdentity() {
    unsigned int i, id;
    double *data = d_data.get();
    memset(static_cast<void *>(data),0,d_dataSize*sizeof(double));
    for (i = 0; i < DIM_2D; i++) {
      id = i*(DIM_2D+1);
      data[id] = 1.0;
    }
  }
  
  void Transform2D::TransformPoint(Point2D &pt) const {
    double x, y;
    double *data = d_data.get();
    x = data[0]*pt.x + data[1]*pt.y + data[2];
    y = data[3]*pt.x + data[4]*pt.y + data[5];
        
    pt.x = x;
    pt.y = y;
  }

  void Transform2D::SetTranslation(const Point2D &pt) {
    unsigned int i = DIM_2D -1;
    double *data = d_data.get();
    data[i] = pt.x; i += DIM_2D;
    data[i] = pt.y; i += DIM_2D;
    data[i] = 1.0; i += DIM_2D;
  }

  void Transform2D::SetTransform(const Point2D &pt, double angle) {
    this->setToIdentity();
    
    Transform2D trans1;
    trans1.SetTranslation(-pt);
    double *data = d_data.get();
    // set the rotation
    data[0] = cos(angle);
    data[1] = -sin(angle);
    data[3] = sin(angle);
    data[4] = cos(angle);
    
    (*this) *= trans1;

    // translation back to the orignal coordinate
    Transform2D trans2;
    trans2.SetTranslation(pt);
    trans2 *= (*this);

    // now combine them
    this->assign(trans2);
  }
    
    
  void Transform2D::SetTransform(const Point2D &ref1, const Point2D &ref2,
                                 const Point2D &pt1, const Point2D &pt2) {
    // compute the angle between the two vectors
    Point2D rvec = ref2 - ref1;
    Point2D pvec = pt2 - pt1;

    double dp = rvec.dotProduct(pvec);
    double lp = (rvec.length())*(pvec.length());
    CHECK_INVARIANT(lp > 0.0, "");
    double cval = dp/lp;
    if (cval < -1.0) {
      cval = -1.0;
    } else if (cval > 1.0) {
      cval = 1.0;
    }

    double ang = acos(cval);
    
    // figure out if we have to do clock wise or anti clock wise rotation
    double cross = (pvec.x)*(rvec.y) - (pvec.y)*(rvec.x);
    
    if (cross < 0.0) { 
      ang *= -1.0;
    }
    this->setToIdentity();
    // set the rotation 
    double *data = d_data.get();
    data[0] = cos(ang);
    data[1] = -sin(ang);
    data[3] = sin(ang);
    data[4] = cos(ang);
    
    // apply this rotation to pt1 and compute the translation
    Point2D npt1 = pt1;
    this->TransformPoint(npt1);
    data[DIM_2D-1] = ref1.x - npt1.x;
    data[2*DIM_2D-1] = ref1.y - npt1.y;
    
  }
  
}

RDGeom::Transform2D operator* (const RDGeom::Transform2D &t1, const RDGeom::Transform2D &t2) {
  RDGeom::Transform2D res;
  RDNumeric::multiply(t1, t2, res);
  return res;
};