/*//////////////////////////////////////////////////////////////////////////////
// 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/ContainerBox.h"

#include <cmath>
#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 ContainerBox);

  {
    ContainerBox Cnt;
    Cnt.SetOrigin(Vector3f(0,0,0));
    Cnt.SetSize(Vector3f(2,2,2));
    TEST0( Vector4f0(Cnt.GetOrigin().homogeneous() - HVector3f(0,0,0)) );
    TEST0( Vector4f0(Cnt.GetSize().homogeneous() - HVector3f(2,2,2)) );


    HPoint3f pt = Cnt.GetLocalPoint(HPoint3f(0,0,0));
    HPoint3f wp = Cnt.GetWorldPoint(pt);  
    TEST0( Vector4f0(wp - HPoint3f(0,0,0)) );

    HPoint3f pt2 = Cnt.GetLocalPoint(HPoint3f(2,2,2));
    HPoint3f wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(2,2,2)) );

    HPoint3f pt3 = Cnt.GetLocalPoint(HPoint3f(1,1,1));
    HPoint3f wp3 = Cnt.GetWorldPoint(pt3);  
    TEST0( Vector4f0(wp3 - HPoint3f(1,1,1)) );

    HPoint3f pt4 = Cnt.GetLocalPoint(HPoint3f(1,2,3));
    HPoint3f wp4 = Cnt.GetWorldPoint(pt4);  
    TEST0( Vector4f0(wp4 - HPoint3f(1,2,3)) );
  }

  {
    ContainerBox Cnt;
    Cnt.SetOrigin(Vector3f(0,0,0));
    Cnt.SetSize(Vector3f(2,2,2));
    Cnt.EulerYZYRotate(Vector3f(M_PI,0,0));
    
    HPoint3f pt = Cnt.GetLocalPoint(HPoint3f(0,0,0));
    HPoint3f wp = Cnt.GetWorldPoint(pt);  
    TEST0( Vector4f0(wp - HPoint3f(0,0,0)) );

    HPoint3f pt2 = Cnt.GetLocalPoint(HPoint3f(2,2,2));
    HPoint3f wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(2,2,2)) );

    pt2 = HPoint3f(1,1,1);
    wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(-2,2,-2)) );

    pt2 = HPoint3f(1,2,3);
    wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(-2,4,-6)) );
  }

  {
    ContainerBox Cnt;
    Cnt.SetOrigin(Vector3f(-1,-1,-1));
    Cnt.SetSize(Vector3f(2,2,2)); // scaling //

    HPoint3f pt2 = HPoint3f(.5,.5,.5);
    HPoint3f wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(0,0,0)) );

    pt2 = HPoint3f(0,0,0);
    wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(-1,-1,-1)) );

    Cnt.EulerYZYRotate(Vector3f(M_PI,0,0));
    pt2 = HPoint3f(0,0,0);
    wp2 = Cnt.GetWorldPoint(pt2);  
    TEST0( Vector4f0(wp2 - HPoint3f(1,-1,1)) );
  }

  {
    ContainerBox Box;
    Box.SetOrigin(Vector3f(1,1,1));
    Box.SetSize(Vector3f(2,2,2));
    Box.EulerYZYRotate(Vector3f(0,0,0));
    HPoint3f pt = Box.GetLocalPoint(HPoint3f(2,3,2));
    HPoint3f wp = Box.GetWorldPoint(pt);
    TEST0( Vector4f0(wp - HPoint3f(2,3,2)) );
  }

  {
    ContainerBox Box;
    Box.SetPosition(Vector3f(-0.5,-0.5,-0.5));
    Box.SetSize(Vector3f(1,1,1));
    HPoint3f pt = Box.GetLocalPoint(HPoint3f(0,0,0));
    HPoint3f wp = Box.GetWorldPoint(pt);
    TEST0( Vector4f0(wp - HPoint3f(0,0,0)) );
  }

  {
    ContainerBox Box;
    Box.SetOrigin(Vector3f(-1.5,-1.5,-1.5));
    Box.SetSize(Vector3f(3,3,3));
    HPoint3f pt = Box.GetLocalPoint(HPoint3f(0,0,0));
    HPoint3f wp = Box.GetWorldPoint(pt);
    TEST0( Vector4f0(wp - HPoint3f(0,0,0)) );

    pt = HPoint3f(1,1,1);
    wp = Box.GetWorldPoint(pt);
    TEST0( Vector4f0(wp - HPoint3f(1.5, 1.5, 1.5)) ); 

    Box.EulerYZYRotate(Vector3f(M_PI,0,0));
    pt = HPoint3f(1,1,1);
    wp = Box.GetWorldPoint(pt);
    TEST0( Vector4f0(wp - HPoint3f(-1.5, 1.5, -1.5)) ); 

  }






  END_TESTING;
}