/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis 
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <iostream>
#include <sstream> 
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

#include <mia/3d/rot.hh>
#include <mia/core/attributes.cxx>

NS_MIA_BEGIN

using std::string; 
using std::istringstream; 
using std::ostringstream; 
using std::vector; 
using std::invalid_argument; 

static const char * c_rot_identity = "rot-identity"; 
static const char * c_rot_mat = "rot-matrix"; 
static const char * c_rot_quat = "rot-quaternion"; 



class C3DRotationImpl {
        
public: 
	virtual ~C3DRotationImpl(); 
	virtual C3DRotationImpl *clone() const  __attribute__((warn_unused_result))= 0 ; 
        virtual C3DDMatrix as_matrix_3x3() const = 0;
        virtual Quaternion as_quaternion() const = 0; 
        virtual std::string as_string() const = 0; 
        
        static C3DRotationImpl* from_string(const std::string& s) __attribute__((warn_unused_result));
}; 

class C3DQuaternionRotation: public C3DRotationImpl {

public: 
        C3DQuaternionRotation(const std::string& s); 
        C3DQuaternionRotation(const Quaternion& q); 

	
	virtual C3DRotationImpl *clone() const  __attribute__((warn_unused_result)); 
        virtual C3DDMatrix as_matrix_3x3() const;
        virtual Quaternion as_quaternion() const; 
        virtual std::string as_string() const; 

private: 
        Quaternion m_q;
        
}; 

class C3DMatrix3x3Rotation: public C3DRotationImpl {
public: 
        C3DMatrix3x3Rotation(const std::string& s);
        C3DMatrix3x3Rotation(const C3DDMatrix& q); 
        
	virtual C3DRotationImpl *clone() const  __attribute__((warn_unused_result)); 
        virtual C3DDMatrix as_matrix_3x3() const;
        virtual Quaternion as_quaternion() const; 
        virtual std::string as_string() const; 

private: 

        C3DDMatrix m_matrix;
}; 

C3DRotation::C3DRotation():impl(nullptr)
{
}

C3DRotation::~C3DRotation()
{
	delete impl; 
}

C3DRotation::C3DRotation(const C3DRotation& other)
{
	if (other.impl) 
		impl = other.impl->clone(); 
	else 
		impl = nullptr; 
}

C3DRotation& C3DRotation::operator = (const C3DRotation& other)
{
	if (this != &other) {
		auto old = impl; 
		if (other.impl) 
			impl = other.impl->clone(); 
		else 
			impl = nullptr;
		delete old; 
	}
	return *this; 
}

C3DRotation::C3DRotation(const C3DDMatrix& m)
{
	impl = new C3DMatrix3x3Rotation(m); 
}

C3DRotation::C3DRotation(const Quaternion& q)
{
	impl = new C3DQuaternionRotation(q); 
}
	
C3DRotation::C3DRotation(const std::string& s)
{
	impl = C3DRotationImpl::from_string(s); 
}

C3DDMatrix C3DRotation::as_matrix_3x3() const
{
	return impl ? impl->as_matrix_3x3() : C3DDMatrix::_1; 
}

Quaternion C3DRotation::as_quaternion() const
{
	return impl ? impl->as_quaternion() : Quaternion::_1; 
}

string C3DRotation::as_string() const
{
	return impl ? impl->as_string() : string(c_rot_identity); 
}

const C3DRotation C3DRotation::_1; 

C3DRotationImpl::~C3DRotationImpl()
{
}

C3DRotationImpl* C3DRotationImpl::from_string(const std::string& s) 
{
        vector<string> tockens; 
        boost::split(tockens, s ,boost::is_any_of("="));

	if ((tockens.size() == 1) && !strcmp(c_rot_identity, tockens[0].c_str()))
		return nullptr;
	
        if (tockens.size() != 2) {
                throw create_exception<invalid_argument>("Unable to read C3DRotation from '", 
                                                         s, "'"); 
        }
        
        const char *test_tocken =  tockens[0].c_str(); 
        if (!strcmp(c_rot_mat, test_tocken))
                return new C3DMatrix3x3Rotation(tockens[1]); 
        else if (!strcmp(c_rot_quat, test_tocken))
                return new C3DQuaternionRotation(tockens[1]);
	else
                throw create_exception<invalid_argument>("Unknown C3DRotation type '", tockens[0], "'"); 
}

C3DRotationImpl *C3DQuaternionRotation::clone() const 
{
	return new C3DQuaternionRotation(*this); 
}

C3DQuaternionRotation::C3DQuaternionRotation(const std::string& s)
{
        vector<string> tockens; 
        boost::split(tockens, s ,boost::is_any_of(","));
        
        if (tockens.size() != 4) {
                throw create_exception<invalid_argument>("Expect four values to create quaternions but got ", tockens.size()); 
        }
        
        double v[4]; 
        for (int i = 0; i < 4; ++i) {
                istringstream iss(tockens[i]); 
                iss >> v[i]; 
                if (iss.bad()) 
                        throw create_exception<invalid_argument>("Unable to read quaternions from '", s, "'"); 
        }
        m_q = Quaternion(v[0], v[1], v[2], v[3]); 
}

C3DQuaternionRotation::C3DQuaternionRotation(const Quaternion& q):
      m_q(q)
{
        
}

C3DDMatrix C3DQuaternionRotation::as_matrix_3x3() const
{
        return m_q.get_rotation_matrix(); 
}

Quaternion C3DQuaternionRotation::as_quaternion() const
{
        return m_q; 
}

std::string C3DQuaternionRotation::as_string() const
{
        ostringstream s; 
        s << c_rot_quat << "=" <<m_q; 
        return s.str(); 
}

C3DRotationImpl *C3DMatrix3x3Rotation::clone()const 
{
	return new C3DMatrix3x3Rotation(*this); 
}

C3DMatrix3x3Rotation::C3DMatrix3x3Rotation(const std::string& s)
{
        vector<string> tockens; 
        boost::split(tockens, s ,boost::is_any_of(";"));
	
        if (tockens.size() != 3) {
                throw create_exception<invalid_argument>("Unable to read C3DMatrix3x3Rotation from '", 
                                                         s, "'"); 
        }

        istringstream iss_x(tockens[0]); 
        iss_x >> m_matrix.x; 
        if (iss_x.bad()) {
                throw create_exception<invalid_argument>("Unable to rotation matrix x row from '", tockens[0], "'");
        }

        istringstream iss_y(tockens[1]); 
        iss_y >> m_matrix.y; 
        if (iss_y.bad()) {
                throw create_exception<invalid_argument>("Unable to rotation matrix y row from '", tockens[1], "'");
        }

        istringstream iss_z(tockens[2]); 
        iss_z >> m_matrix.z; 
        if (iss_z.bad()) {
                throw create_exception<invalid_argument>("Unable to rotation matrix z row from '", tockens[2], "'");
        }

}

C3DMatrix3x3Rotation::C3DMatrix3x3Rotation(const C3DDMatrix& m):
m_matrix(m)
{
        
}


C3DDMatrix C3DMatrix3x3Rotation::as_matrix_3x3() const
{
        return m_matrix; 
}

Quaternion C3DMatrix3x3Rotation::as_quaternion() const
{
        return Quaternion(m_matrix); 
}

std::string C3DMatrix3x3Rotation::as_string() const
{
        ostringstream s; 
        s << c_rot_mat << "="<< m_matrix; 
        return s.str(); 
}

EXPORT_3D bool operator == (const C3DRotation& lhs, const C3DRotation& rhs)
{
	// this needs to be done floating point value friendly
	auto q = lhs.as_quaternion(); 
	q -= rhs.as_quaternion(); 
	
	return (q.norm() < 1e-10); 
}

EXPORT_3D bool operator < (const C3DRotation& lhs, const C3DRotation& rhs) 
{
	auto qlhs = lhs.as_quaternion(); 
	auto qrhs = rhs.as_quaternion(); 

	return qlhs.w() < qrhs.w() || 
		(qlhs.w() == qrhs.w() && 
		 (qlhs.z() < qrhs.z() || 
		  (qlhs.z() == qrhs.z() && 
		   ( qlhs.y() < qrhs.y() || 
		     ( qlhs.y() == qrhs.y() && qlhs.x() < qrhs.x()))))); 
		  
}

template class EXPORT_3D TAttribute<C3DRotation>; 
template class EXPORT_3D TTranslator<C3DRotation>;

NS_MIA_END
