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


namespace uLib {

void QuadMesh::PrintSelf(std::ostream &o)
{
    o << " // ------- QUAD MESH ------- // \n" ;
    o << "    #Points : " << m_Points.size() << "\n";
    o << "    #Quads  : " << m_Quads.size() << "\n";
    for(int i=0; i < m_Quads.size(); ++i ) {
        o << "     - quad[" << i << "]" <<
             " " << m_Quads[i](0) << "->(" << GetPoint(m_Quads[i](0)).transpose() << ") " <<
             " " << m_Quads[i](1) << "->(" << GetPoint(m_Quads[i](1)).transpose() << ") " <<
             " " << m_Quads[i](2) << "->(" << GetPoint(m_Quads[i](2)).transpose() << ") " <<
             " " << m_Quads[i](3) << "->(" << GetPoint(m_Quads[i](3)).transpose() << ") " <<
             " \n";
    }
    o << " // ------------------------- // \n";
}

void QuadMesh::AddPoint(const Vector3f &pt)
{
    Vector4f p(pt.x(), pt.y(), pt.z(), 1.0f);
    Vector4f localP = this->GetWorldMatrix().inverse() * p;
    this->m_Points.push_back(localP.head<3>());
}

Vector3f QuadMesh::GetPoint(const Id_t id) const
{
    Vector4f p(m_Points.at(id).x(), m_Points.at(id).y(), m_Points.at(id).z(), 1.0f);
    Vector4f worldP = this->GetWorldMatrix() * p;
    return worldP.head<3>();
}

void QuadMesh::AddQuad(const Id_t *id)
{
    Vector4i quad(id[0],id[1],id[2],id[3]);
    this->m_Quads.push_back(quad);
}

void QuadMesh::AddQuad(const Vector4i &id)
{
    this->m_Quads.push_back(id);
}

Vector3f QuadMesh::GetNormal(const Id_t id) const
{
    const Vector4i &quad = m_Quads.at(id);
    const Vector3f v0 = this->GetPoint(quad(0));
    const Vector3f v1 = this->GetPoint(quad(1));
    const Vector3f v3 = this->GetPoint(quad(3));
    
    Vector3f edge1 = v1 - v0;
    Vector3f edge2 = v3 - v0;
    
    return edge1.cross(edge2).normalized();
}


}