/*//////////////////////////////////////////////////////////////////////////////
// CMT Cosmic Muon Tomography project //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

  Copyright (c) 2014, Universita' degli Studi di Padova, INFN sez. di Padova
  All rights reserved

  Authors: Andrea Rigoni Garola < andrea.rigoni@pd.infn.it >

  ------------------------------------------------------------------
  This library is free software;  you  can  redistribute  it  and/or
  modify it  under the  terms  of  the  GNU  Lesser  General  Public
  License as published  by  the  Free  Software  Foundation;  either
  version 3.0 of the License, or (at your option) any later version.

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of  the GNU Lesser General  Public
  License along with this library.

//////////////////////////////////////////////////////////////////////////////*/



#ifndef U_GEOMETRY_H
#define U_GEOMETRY_H

#include "Core/Object.h"
#include "Math/Dense.h"
#include "Math/Transform.h"
#include <cmath>

namespace uLib {

class Geometry : public AffineTransform, public Object {
public:

    virtual const char * GetClassName() const { return "Geometry"; }

    virtual Vector3f ToLinear(const Vector3f& curved_space) const {
        return curved_space;
    }

    virtual Vector3f FromLinear(const Vector3f& cartesian_space) const {
        return cartesian_space;
    }

    inline Vector4f GetWorldPoint(const Vector4f v) const {
        Vector3f lin = ToLinear(Vector3f(v.x(), v.y(), v.z()));
        return this->GetWorldMatrix() * Vector4f(lin.x(), lin.y(), lin.z(), v.w());
    }

    inline Vector4f GetWorldPoint(const float x, const float y, const float z) {
        return this->GetWorldPoint(Vector4f(x,y,z,1));
    }

    inline Vector4f GetLocalPoint(const Vector4f v) const {
        Vector4f loc_lin = this->GetWorldMatrix().inverse() * v;
        Vector3f curv = FromLinear(Vector3f(loc_lin.x(), loc_lin.y(), loc_lin.z()));
        return Vector4f(curv.x(), curv.y(), curv.z(), loc_lin.w());
    }

    inline Vector4f GetLocalPoint(const float x, const float y, const float z) {
         return this->GetLocalPoint(Vector4f(x,y,z,1));
    }
};

class CylindricalGeometry : public Geometry {
public:
    CylindricalGeometry() {}

    Vector3f ToLinear(const Vector3f& cylindrical) const {
        return Vector3f(cylindrical.x() * std::cos(cylindrical.y()), 
                        cylindrical.x() * std::sin(cylindrical.y()), 
                        cylindrical.z());
    }

    Vector3f FromLinear(const Vector3f& linear) const {
        float r = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y());
        float phi = std::atan2(linear.y(), linear.x());
        return Vector3f(r, phi, linear.z());
    }

};

class SphericalGeometry : public Geometry {
public:
    SphericalGeometry() {}

    virtual const char * GetClassName() const { return "SphericalGeometry"; }

    Vector3f ToLinear(const Vector3f& spherical) const {
        float r = spherical.x();
        float theta = spherical.y();
        float phi = spherical.z();
        return Vector3f(r * std::sin(theta) * std::cos(phi),
                        r * std::sin(theta) * std::sin(phi),
                        r * std::cos(theta));
    }

    Vector3f FromLinear(const Vector3f& linear) const {
        float r = linear.norm();
        float theta = (r == 0.0f) ? 0.0f : std::acos(linear.z() / r);
        float phi = std::atan2(linear.y(), linear.x());
        return Vector3f(r, theta, phi);
    }

};

class ToroidalGeometry : public Geometry {
public:
    ToroidalGeometry(float Rtor) : m_Rtor(Rtor) {}

    virtual const char * GetClassName() const { return "ToroidalGeometry"; }

    Vector3f ToLinear(const Vector3f& toroidal) const {
        float r = toroidal.x();
        float theta = toroidal.y();
        float phi = toroidal.z();
        return Vector3f((m_Rtor + r * std::cos(theta)) * std::cos(phi),
                        (m_Rtor + r * std::cos(theta)) * std::sin(phi),
                        r * std::sin(theta));
    }

    Vector3f FromLinear(const Vector3f& linear) const {
        float phi = std::atan2(linear.y(), linear.x());
        float r_xy = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y());
        float delta_r = r_xy - m_Rtor;
        float z = linear.z();
        float r = std::sqrt(delta_r * delta_r + z * z);
        float theta = std::atan2(z, delta_r);
        return Vector3f(r, theta, phi);
    }

private:
    float m_Rtor;

};

}

#endif // GEOMETRY_H