#include "gpklib/assert.h"
#include "gpklib/m22v2.h"
#include "gpklib/ltrans.h"
#include "gpklib/box.h"

ltransform::ltransform(C matrix22&w, C vector2& s)
 : warp(w), shift(s)
 {;}

vector2 ltransform::operator()(C vector2& a) C
{
 return warp*a + shift;
}

ltransform ltransform::operator()(C ltransform& x) C
{
 return ltransform(warp*x.warp, warp*x.shift+shift);
}

ltransform inverse(C ltransform& x)
{
 C matrix22 iw = ::inverse(x.warp);

 return ltransform(x.shift, x.Center(), iw);
}

ltransform sqrt(C ltransform& x)
{
 C matrix22 w = sqrt(x.warp);

 return ltransform(w, ::inverse(w+matrix22::identity())*x.shift);
}

ostream& operator<<(ostream& s, C ltransform& t)
{
 return s << "{" << t.shift << "+" << t.warp << "*}";
}

ltransform ltransform::identity(C vector_space *sfrom, C vector_space *sto)
{
 return ltransform(vector2(0.0, 0.0, sfrom), vector2(0.0, 0.0, sto),
					matrix22::identity());
}

ltransform::ltransform(C vector2& initial_point, C vector2& final_point,
											C matrix22& _warp)
	/* implementing final_point + warp*(x-initial_point) */
 : warp(_warp), shift(final_point - _warp*initial_point)
{
 warp.in = vector_space::vseq(warp.in, initial_point.space());
 warp.out = vector_space::vseq(warp.out, final_point.space());
 assert(vector_space::vsok(warp.in));
 assert(vector_space::vsok(warp.out));
}

ltransform::ltransform(C box& from, C box& to)
 : warp(matrix22(to.size()(0)/from.size()(0), 0,
					0, to.size()(1)/from.size()(1),
					from.c00().space(), to.c00().space())),
	shift(to.c00() - warp*from.c00())
{}

line ltransform::operator()(C line& a) C
{
 return line((*this)(a.base()), (*this)(a.base()+a.dir()));
}

ltransform ltransform::translate(C vector2& s)
{
 return ltransform(matrix22::identity(), s);
}

ltransform::ltransform()
: transform(), warp(), shift()
{}

ltransform::ltransform(C ltransform& a)
: transform(), warp(a.warp), shift(a.shift)
{}

BOOL ltransform::ok() C
{
 return warp.ok() && shift.ok()
		&& vector_space::vseQ(warp.out, shift.space());
}

vector2 ltransform::inverse(C vector2& x) C
{return ::inverse(*this)(x);}

ltransform& ltransform::operator=(C ltransform& a)
{
	if(&a != this) {
		warp = a.warp;
		shift = a.shift;
		}
	return *this;
}

ltransform::~ltransform() {}
