/*//////////////////////////////////////////////////////////////////////////////
// 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.

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

#include "testing-prototype.h"

#include "Math/Dense.h"
#include "Math/Cylinder.h"

#include <cmath>
#include <iostream>

using namespace uLib;

/**
 * @brief Utility function to check if a 4D vector (spatial part) is zero within a threshold.
 * Returns 0 if zero, 1 otherwise.
 */
int Vector4f0(Vector4f c)
{
  c(3) = 0;
  if ( fabs(c(0)) < 0.001 && fabs(c(1)) < 0.001 && fabs(c(2)) < 0.001 )
    return 0;
  else
    return 1;
}

int main()
{
  BEGIN_TESTING(Math Cylinder);

  // Test 1: Basic identity transformation and cylinder parameters
  {
    Cylinder cyl(2.0, 10.0, 2);
    std::cout << "Cyl R=" << cyl.GetRadius() << " H=" << cyl.GetHeight() << std::endl;
    std::cout << "Cyl World Matrix:\n" << cyl.GetWorldMatrix() << std::endl;

    TEST0( std::abs(cyl.GetRadius() - 2.0) > 0.001 );
    TEST0( std::abs(cyl.GetHeight() - 10.0) > 0.001 );

    // Point on the base circle center (Origin)
    Vector4f p0 = cyl.GetWorldPoint(0, 0, 0);
    std::cout << "p0: " << p0.transpose() << std::endl;
    TEST0( Vector4f0(p0 - Vector4f(0, 0, 0, 1)) );

    // Point on the top circle center (0, 0, Height)
    Vector4f p1 = cyl.GetWorldPoint(0, 0, 10.0);
    std::cout << "p1: " << p1.transpose() << std::endl;
    TEST0( Vector4f0(p1 - Vector4f(0, 0, 10.0, 1)) );

    // Point on the edge of the base circle at theta=0 (Radius, 0, 0)
    Vector4f p2 = cyl.GetWorldPoint(2.0, 0, 0);
    std::cout << "p2: " << p2.transpose() << std::endl;
    TEST0( Vector4f0(p2 - Vector4f(2.0, 0, 0, 1)) );

    // Point at 90 degrees on the side wall at middle height
    Vector4f p3 = cyl.GetWorldPoint(2.0, M_PI/2.0, 5.0);
    std::cout << "p3: " << p3.transpose() << std::endl;
    TEST0( Vector4f0(p3 - Vector4f(0, 2.0, 5.0, 1)) );
  }

  // Test 2: Translation
  {
    Cylinder cyl(1.0, 2.0, 2);
    cyl.SetPosition(Vector3f(10, 20, 30));
    
    // Local base origin (0, 0, 0) -> World (10, 20, 30)
    Vector4f p0 = cyl.GetWorldPoint(0, 0, 0);
    TEST0( Vector4f0(p0 - Vector4f(10, 20, 30, 1)) );

    // Local top edge (1, 0, 2) -> World (11, 20, 32)
    Vector4f p1 = cyl.GetWorldPoint(1, 0, 2);
    TEST0( Vector4f0(p1 - Vector4f(11, 20, 32, 1)) );
  }

  // Test 3: Rotation and complex mapping
  {
    Cylinder cyl(5.0, 20.0, 2);
    cyl.SetPosition(Vector3f(1.0, 2.0, 3.0));
    // Rotate 90 degrees around X: Local Y becomes World Z, Local Z becomes World -Y
    cyl.Rotate(M_PI/2.0, Vector3f(1, 0, 0));
    
    // Let's take a local point: r=5, theta=pi/2, z=10 -> (0, 5, 10) in local cartesian
    // Transformed: 
    // Position: (1, 2, 3)
    // Point relative to position: (0, -10, 5)  [since Z local -> -Y world, Y local -> Z world]
    // Final World: (1, 2-10, 3+5) = (1, -8, 8)
    Vector4f world_p = cyl.GetWorldPoint(5.0, M_PI/2.0, 10.0);
    TEST0( Vector4f0(world_p - Vector4f(1.0, -8.0, 8.0, 1)) );

    // Test inverse mapping
    Vector3f cyl_local = cyl.GetCylindricalLocal(world_p);
    TEST0( std::abs(cyl_local.x() - 5.0) > 0.001 );
    TEST0( std::abs(cyl_local.y() - M_PI/2.0) > 0.001 );
    TEST0( std::abs(cyl_local.z() - 10.0) > 0.001 );
  }

  END_TESTING;
}