/*//////////////////////////////////////////////////////////////////////////////
// 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/Geometry.h"
#include "Math/ContainerBox.h"

#include <iostream>
#include <math.h>

using namespace uLib;

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 Geometry);

  //////////////////////////////////////////////////////////////////////////////
  ///////////////// GEOMETRY TESTING ///////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////

  LinearGeometry Geo;

  Geo.SetPosition(Vector3f(1,1,1));
  Geo.EulerYZYRotate(Vector3f(0,0,0));
  HPoint3f pt = Geo.GetLocalPoint(HPoint3f(2,3,2));
  HPoint3f wp = Geo.GetWorldPoint(pt);
  TEST0( Vector4f0(wp - HPoint3f(2,3,2)) );

  Geo.Scale(Vector3f(2,2,2));
  wp = Geo.GetWorldPoint(HPoint3f(1,1,1));
  TEST0( Vector4f0(wp - HPoint3f(3,3,3)) );
  Geo.Scale(Vector3f( .5 , .5 , .5 ));


  Geo.EulerYZYRotate(Vector3f(M_PI_2,0,0));
  wp = Geo.GetWorldPoint(HPoint3f(1,1,1));
  TEST0( Vector4f0(wp - HPoint3f(2,2,0)) );

  Geo.EulerYZYRotate(Vector3f(0,M_PI_2,0));
  wp = Geo.GetWorldPoint(HPoint3f(1,1,1));
  TEST0( Vector4f0(wp - HPoint3f(2,2,2)) );

  Geo.EulerYZYRotate(Vector3f(0,0,M_PI_2));
  wp = Geo.GetWorldPoint(HPoint3f(1,1,1));
  //  std::cout << "Geometry matrix\n" << Geo.GetTransform().matrix() << "\n";
  //  std::cout << "World 1,1,1 coords\n" << wp << "\n";
  TEST0( Vector4f0(wp - HPoint3f(0,2,2)) );

  // TESTING FLIP AXES //

  Geo.SetPosition(Vector3f(0,0,0));
  Geo.EulerYZYRotate(Vector3f(-M_PI_2,-M_PI_2,-M_PI_2)); // reset previous
  Geo.EulerYZYRotate(Vector3f(M_PI_2,0,0));              // PI_2 along X
  Geo.FlipAxes(0,2); // flip  X-Z
  HPoint3f p = Geo.GetWorldPoint(Vector3f(1,0,0).homogeneous());
  TEST0( Vector4f0(p - HVector3f(1,0,0)) ); // after flip and rotation X->X



  // CYLINDRICAL GEOMETRY TESTING
  {
    CylindricalGeometry cyl;
    Vector3f cyl_pt(5.0f, M_PI_2, 3.0f);
    Vector3f lin = cyl.ToLinear(cyl_pt);
    TEST0( Vector4f0(lin.homogeneous() - HPoint3f(0.0f, 5.0f, 3.0f)) );
    Vector3f recovered = cyl.FromLinear(lin);
    TEST0( Vector4f0(recovered.homogeneous() - cyl_pt.homogeneous()) );
  }

  // SPHERICAL GEOMETRY TESTING
  {
    SphericalGeometry sph;
    Vector3f sph_pt(5.0f, M_PI_2, M_PI);
    Vector3f lin = sph.ToLinear(sph_pt);
    TEST0( Vector4f0(lin.homogeneous() - HPoint3f(-5.0f, 0.0f, 0.0f)) );
    Vector3f recovered = sph.FromLinear(Vector3f(-5.0f, 0.0f, 0.0f));
    TEST0( Vector4f0(recovered.homogeneous() - sph_pt.homogeneous()) );
  }

  // TOROIDAL GEOMETRY TESTING
  {
    ToroidalGeometry tor(10.0f);
    Vector3f tor_pt(1.0f, M_PI_2, M_PI);
    Vector3f lin = tor.ToLinear(tor_pt);
    TEST0( Vector4f0(lin.homogeneous() - HPoint3f(-10.0f, 0.0f, 1.0f)) );
    Vector3f recovered = tor.FromLinear(Vector3f(-10.0f, 0.0f, 1.0f));
    TEST0( Vector4f0(recovered.homogeneous() - tor_pt.homogeneous()) );
  }

  // PARENT GEOMETRY TESTING
  {
    LinearGeometry parent;
    parent.Translate(Vector3f(10, 0, 0));

    LinearGeometry child;
    child.SetParent(&parent);
    child.Translate(Vector3f(0, 5, 0));

    HPoint3f wp = child.GetWorldPoint(HPoint3f(1, 1, 1));
    TEST0( Vector4f0(wp - HPoint3f(11, 6, 1)) );

    CylindricalGeometry cparent;
    LinearGeometry grandchild;
    grandchild.SetParent(&cparent);
    grandchild.Translate(Vector3f(1, 0, 0)); 

    HPoint3f gp = grandchild.GetWorldPoint(HPoint3f(1, M_PI_2, 0));
    TEST0( Vector4f0(gp - HPoint3f(0, 2, 0)) );
  }

  END_TESTING;
}