///
// Copyright (C) 2002 - 2004, Fredrik Arnerup & Rasmus Kaj, See COPYING
///
#include "matrix.h"
#include "typeinfo.h"
#include <libgnomecanvasmm/affinetrans.h>
#include <iostream>
#include <cmath>

template<> std::string TypeInfo<Matrix>::name() { return "transform matrix"; }

template<class C>
inline C abs(const C& c) { return c < 0 ? -c : c; }

Matrix::Matrix() {
  // unity matrix
  rep[0] = rep[3] = 1;
  rep[1] = rep[2] = rep[4] = rep[5] = 0;
}

Matrix Matrix::rotation(double angle) {
  Matrix tmp;
  tmp.rep[3] = tmp.rep[0] = cos(angle);
  tmp.rep[2] = -(tmp.rep[1] = sin(angle));
  return tmp;
}

Matrix Matrix::scaling(double xfactor, double yfactor) {
  Matrix tmp;
  tmp.rep[0] = xfactor;
  tmp.rep[3] = yfactor;
  return tmp;
}

Matrix Matrix::translation(const Vector& offset) {
  Matrix tmp;
  tmp.rep[4] = offset.x;
  tmp.rep[5] = offset.y;
  return tmp;
}

Matrix Matrix::translation(double xoffset, double yoffset) {
  Matrix tmp;
  tmp.rep[4] = xoffset;
  tmp.rep[5] = yoffset;
  return tmp;
}

Matrix Matrix::shearing(double shcoeff) {
  Matrix tmp;
  tmp.rep[2] = tan(shcoeff);
  return tmp;
}

const Gnome::Art::AffineTrans Matrix::gaTransform() const {
  using Gnome::Art::AffineTrans;
  return (AffineTrans::shearing(-rad2deg(sh()))
	  * AffineTrans::scaling(sc_x(), sc_y())
	  * AffineTrans::rotation(-rad2deg(rot()))
	  * AffineTrans::translation(tr_x(), -tr_y()));
}

bool Matrix::is_plain() const {
  const float epsilon = 1e-5;
  return (abs(rep[2]) <= epsilon && abs(rep[1]) <= epsilon &&
	  abs(rep[0]-1) <= epsilon && abs(rep[3]-1) <= epsilon);
}

double Matrix::tr_x() const {
  return rep[4];
}

double Matrix::tr_y() const {
  return rep[5];
}

Vector Matrix::tr() const {
  return Vector(tr_x(), tr_y());
}

double Matrix::rot() const {
  if(a() != 0)
    return atan2(b(), a());
  else if(b() != 0)
    return M_PI / 2;
  else
    throw Error::SingularMatrix("Calculating rotation");
}

double Matrix::sc_x() const {
  if(a() != 0)
    return a() / cos(rot());
  else if(b() != 0)
    return b() / sin(rot());
  else 
    throw Error::SingularMatrix("Calculating x scaling");
}

double Matrix::sc_y() const {
  double scx = sc_x();
  if(scx != 0)
    return (a() * d() - b() * c()) / scx;
  else if(c() != 0)
    return -c() / sin(rot());
  else if(d() != 0)
    return d() / cos(rot());
  else
    throw Error::SingularMatrix("Calculating y scaling");
}

double Matrix::sh() const {
  if(a() != 0)
    return atan2(c() + sc_y() * sin(rot()), a());
  else // if(b() != 0) an exception should occur before we get division by zero
    return atan2(d() - sc_y() * cos(rot()), b());
}

Matrix& Matrix::set_tr_x(double val) {
  rep[4] = val;
  return *this;
}

Matrix& Matrix::set_tr_y(double val) {
  rep[5] = val;
  return *this;
}

Matrix& Matrix::set_tr(const Vector& offset) {
  rep[4] = offset.x; rep[5] = offset.y;
  return *this;
}

Matrix& Matrix::set_rot(double val) {
  return *this =
    shearing(sh())
    * scaling(sc_x(), sc_y())
    * rotation(val)
    * translation(tr_x(), tr_y());
}

Matrix& Matrix::set_scale(double sc_x, double sc_y) {
  if(sc_x == 0 || sc_y == 0)
    throw Error::SingularMatrix("Setting scale");
  return *this =
    shearing(sh())
    * scaling(sc_x, sc_y)
    * rotation(rot())
    * translation(tr_x(), tr_y());
}

Matrix& Matrix::set_sc_x(double val) {
  if(val == 0)
    throw Error::SingularMatrix("Setting x scale");
  return *this =
    shearing(sh())
    * scaling(val, sc_y())
    * rotation(rot())
    * translation(tr_x(), tr_y());
}

Matrix& Matrix::set_sc_y(double val) {
  if(val == 0)
    throw Error::SingularMatrix("Setting y scale");
  return *this =
    shearing(sh())
    * scaling(sc_x(), val)
    * rotation(rot())
    * translation(tr_x(), tr_y());
}

Matrix& Matrix::set_sh(double val) {
  return *this =
    shearing(val)
    * scaling(sc_x(), sc_y())
    * rotation(rot())
    * translation(tr_x(), tr_y());
}

double Matrix::rad2deg(double rad) {
  return 180.0 * rad / M_PI;
}

double Matrix::deg2rad(double deg) {
  return deg * M_PI / 180.0;
}

Vector Matrix::transform(Vector v) const {
  // (row vector)*(matrix)
  return Vector(v.x * rep[0] + v.y * rep[2] + rep[4], 
		v.x * rep[1] + v.y * rep[3] + rep[5]);
}

Matrix Matrix::operator * (const Matrix& m) const {
  Matrix tmp;
  tmp.rep[0] = rep[0] * m.rep[0] + rep[1] * m.rep[2];
  tmp.rep[1] = rep[0] * m.rep[1] + rep[1] * m.rep[3];

  tmp.rep[2] = rep[2] * m.rep[0] + rep[3] * m.rep[2];
  tmp.rep[3] = rep[2] * m.rep[1] + rep[3] * m.rep[3];

  tmp.rep[4] = rep[4] * m.rep[0] + rep[5] * m.rep[2] + m.rep[4];
  tmp.rep[5] = rep[4] * m.rep[1] + rep[5] * m.rep[3] + m.rep[5];

  return tmp;
}

namespace {
  const double epsilon = 1e-6; /// \note this is rather arbitrary
  bool almost_equal(double a, double b) { return abs(a - b) < epsilon; }
}

bool Matrix::operator == (const Matrix& m) const {
  for(int i = 0; i < 6; ++i)
    if(!almost_equal(rep[i], m.rep[i])) return false;

  return true;
}

double Matrix::det() const {
  // determinant expansion along the third column
  return rep[0] * rep[3] - rep[1] * rep[2];
}

Matrix Matrix::inv() const {
  //adjoint matrix
  //  adj[0]=rep[3];
  //  adj[3]=rep[2];
  //  adj[6]=rep[2]*rep[5]-rep[3]*rep[4];
  //  adj[1]=rep[1];
  //  adj[4]=rep[0];
  //  adj[7]=rep[0]*rep[5]-rep[1]*rep[4];
  //  adj[2]=0;
  //  adj[5]=0;
  //  adj[8]=rep[0]*rep[3]-rep[1]*rep[2]; // = the determinant
  double d = det();
  Matrix tmp;
  if(d == 0) // singular matrix
    throw Error::SingularMatrix("Inverting matrix");
  tmp.rep[0] = rep[3] / d;
  tmp.rep[1] = -rep[1] / d;
  tmp.rep[2] = -rep[2] / d;
  tmp.rep[3] = rep[0] / d;
  tmp.rep[4] = (rep[2] * rep[5] - rep[3] * rep[4]) / d;
  tmp.rep[5] = -(rep[0] * rep[5] - rep[1] * rep[4]) / d;
  return tmp;
}

void Matrix::print() const {
  std::cerr << "[ " << rep[0] << " " << rep [1] << " 0 ]" << std::endl
	    << "[ " << rep[2] << " " << rep [3] << " 0 ]" << std::endl
	    << "[ " << rep[4] << " " << rep [5] << " 1 ]" << std::endl;
}

std::ostream& operator<< (std::ostream& out, const Matrix& m) {
  out << m.rep[0];
  for(int i = 1; i < 6; ++i)
    out << ' ' << m.rep[i];
  return out;
}

std::istream& operator>> (std::istream& in, Matrix& m) {
  for(int i = 0; i < 6; ++i)
    if(!(in >> m.rep[i]))
      throw std::runtime_error("Failed to read number for matrix");
  return in;
}