/*
 * This program source code file is part of KiCad, a free EDA CAD application.
 *
 * Copyright (C) 2015-2016 Mario Luzeiro <mrluzeiro@ua.pt>
 * Copyright (C) 1992-2016 KiCad Developers, see AUTHORS.txt for contributors.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, you may find one here:
 * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
 * or you may search the http://www.gnu.org website for the version 2 license,
 * or you may write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
 */

/**
 * @file  ccontainer2d.cpp
 * @brief
 */

#include "ccontainer2d.h"
#include <vector>
#include <mutex>
#include <boost/range/algorithm/partition.hpp>
#include <boost/range/algorithm/nth_element.hpp>
#include <wx/debug.h>


// /////////////////////////////////////////////////////////////////////////////
// CGENERICCONTAINER2
// /////////////////////////////////////////////////////////////////////////////

CGENERICCONTAINER2D::CGENERICCONTAINER2D( OBJECT2D_TYPE aObjType )
{
    m_bbox.Reset();
}


void CGENERICCONTAINER2D::Clear()
{
    std::lock_guard<std::mutex> lock( m_lock );
    m_bbox.Reset();

    for( LIST_OBJECT2D::iterator ii = m_objects.begin();
         ii != m_objects.end();
         ++ii )
    {
        delete *ii;
    }

    m_objects.clear();
}


CGENERICCONTAINER2D::~CGENERICCONTAINER2D()
{
    Clear();
}




// /////////////////////////////////////////////////////////////////////////////
// CCONTAINER2D
// /////////////////////////////////////////////////////////////////////////////

CCONTAINER2D::CCONTAINER2D() : CGENERICCONTAINER2D( OBJECT2D_TYPE::CONTAINER )
{

}
/*

bool CCONTAINER2D::Intersects( const CBBOX2D &aBBox ) const
{
    return false;
}


bool CCONTAINER2D::Overlaps( const CBBOX2D &aBBox ) const
{
    // NOT IMPLEMENTED
    return false;
}


bool CCONTAINER2D::Intersect( const RAYSEG2D &aSegRay, float *aOutT, SFVEC2F *aNormalOut ) const
{
    if( !m_bbox.Intersect( aSegRay ) )
        return false;

    bool hitted = false;

    for( LIST_OBJECT2D::const_iterator ii = m_objects.begin();
         ii != m_objects.end();
         ii++ )
    {
        const COBJECT2D *object = static_cast<const COBJECT2D *>(*ii);

        float t;
        SFVEC2F hitNormal;
        if( object->Intersect( aSegRay, &t, &hitNormal ) )
            if( (hitted == false) || (t < *aOutT ) )
            {
                hitted = true;
                *aOutT = t;
                *aNormalOut = hitNormal;
            }
    }

    return hitted;
}


INTERSECTION_RESULT CCONTAINER2D::IsBBoxInside( const CBBOX2D &aBBox ) const
{
    return INTERSECTION_RESULT::MISSES;
}


bool CCONTAINER2D::IsPointInside( const SFVEC2F &aPoint ) const
{
    if( !m_bbox.Inside( aPoint ) )
        return false;

    for( LIST_OBJECT2D::const_iterator ii = m_objects.begin();
         ii != m_objects.end();
         ii++ )
    {
        const COBJECT2D *object = static_cast<const COBJECT2D *>(*ii);

        if( object->IsPointInside( aPoint ) )
            return true;
    }

    return false;
}

*/

void CCONTAINER2D::GetListObjectsIntersects( const CBBOX2D & aBBox,
                                             CONST_LIST_OBJECT2D &aOutList ) const
{
    // !TODO:
}


bool CCONTAINER2D::IntersectAny( const RAYSEG2D &aSegRay ) const
{
    // !TODO:
    return false;
}



// /////////////////////////////////////////////////////////////////////////////
// CBVHCONTAINER2D
// /////////////////////////////////////////////////////////////////////////////

CBVHCONTAINER2D::CBVHCONTAINER2D() : CGENERICCONTAINER2D( OBJECT2D_TYPE::BVHCONTAINER )
{
    m_isInitialized = false;
    m_bbox.Reset();
    m_elements_to_delete.clear();
    m_Tree = NULL;
}

/*
bool CBVHCONTAINER2D::Intersects( const CBBOX2D &aBBox ) const
{
    // !TODO: implement the BVH
    return m_bbox.Intersects( aBBox );
}


bool CBVHCONTAINER2D::Overlaps( const CBBOX2D &aBBox ) const
{
    // NOT IMPLEMENTED
    return false;
}


bool CBVHCONTAINER2D::Intersect( const RAYSEG2D &aSegRay,
                                 float *aOutT, SFVEC2F *aNormalOut ) const
{
    // !TODO: implement the BVH

    if( !m_bbox.Intersect( aSegRay ) )
        return false;

    bool hitted = false;

    for( LIST_OBJECT2D::const_iterator ii = m_objects.begin();
         ii != m_objects.end();
         ii++ )
    {
        const COBJECT2D *object = static_cast<const COBJECT2D *>(*ii);

        float t;
        SFVEC2F hitNormal;
        if( object->Intersect( aSegRay, &t, &hitNormal ) )
            if( (hitted == false) || (t < *aOutT ) )
            {
                hitted = true;
                *aOutT = t;
                *aNormalOut = hitNormal;
            }
    }

    return hitted;
}


INTERSECTION_RESULT CBVHCONTAINER2D::IsBBoxInside( const CBBOX2D &aBBox ) const
{
    return INTR_MISSES;
}


bool CBVHCONTAINER2D::IsPointInside( const SFVEC2F &aPoint ) const
{
    // !TODO: implement the BVH

    if( !m_bbox.Inside( aPoint ) )
        return false;

    for( LIST_OBJECT2D::const_iterator ii = m_objects.begin();
         ii != m_objects.end();
         ii++ )
    {
        const COBJECT2D *object = static_cast<const COBJECT2D *>(*ii);

        if( object->IsPointInside( aPoint ) )
            return true;
    }

    return false;
}
*/

void CBVHCONTAINER2D::Clear()
{
    CGENERICCONTAINER2D::Clear();
    destroy();
}

void CBVHCONTAINER2D::destroy()
{
    for( std::list<BVH_CONTAINER_NODE_2D *>::iterator ii = m_elements_to_delete.begin();
         ii != m_elements_to_delete.end();
         ++ii )
    {
        delete *ii;
    }
    m_elements_to_delete.clear();
    m_Tree = nullptr;
    m_isInitialized = false;
}


CBVHCONTAINER2D::~CBVHCONTAINER2D()
{
    destroy();
}


#define BVH_CONTAINER2D_MAX_OBJ_PER_LEAF 4


void CBVHCONTAINER2D::BuildBVH()
{
    if( m_isInitialized )
        destroy();

    m_isInitialized = true;

    if( m_objects.empty() )
    {
        return;
    }

    m_Tree = new BVH_CONTAINER_NODE_2D;

    m_elements_to_delete.push_back( m_Tree );
    m_Tree->m_BBox = m_bbox;

    for( LIST_OBJECT2D::const_iterator ii = m_objects.begin();
         ii != m_objects.end();
         ++ii )
    {
        m_Tree->m_LeafList.push_back( static_cast<const COBJECT2D *>(*ii) );
    }

    recursiveBuild_MIDDLE_SPLIT( m_Tree );
}


// Based on a blog post by VADIM KRAVCENKO
// http://www.vadimkravcenko.com/bvh-tree-building
// Implements:

// "Split in the middle of the longest Axis"
// "Creates a binary tree with Top-Down approach.
//  Fastest BVH building, but least [speed] accuracy."

static bool sortByCentroid_X( const COBJECT2D *a, const COBJECT2D *b )
{
    return a->GetCentroid()[0] < b->GetCentroid()[0];
}

static bool sortByCentroid_Y( const COBJECT2D *a, const COBJECT2D *b )
{
    return a->GetCentroid()[0] < b->GetCentroid()[0];
}

static bool sortByCentroid_Z( const COBJECT2D *a, const COBJECT2D *b )
{
    return a->GetCentroid()[0] < b->GetCentroid()[0];
}

void CBVHCONTAINER2D::recursiveBuild_MIDDLE_SPLIT( BVH_CONTAINER_NODE_2D *aNodeParent )
{
    wxASSERT( aNodeParent != NULL );
    wxASSERT( aNodeParent->m_BBox.IsInitialized() == true );
    wxASSERT( aNodeParent->m_LeafList.size() > 0 );

    if( aNodeParent->m_LeafList.size() > BVH_CONTAINER2D_MAX_OBJ_PER_LEAF )
    {
        // Create Leaf Nodes
        BVH_CONTAINER_NODE_2D *leftNode  = new BVH_CONTAINER_NODE_2D;
        BVH_CONTAINER_NODE_2D *rightNode = new BVH_CONTAINER_NODE_2D;
        m_elements_to_delete.push_back( leftNode );
        m_elements_to_delete.push_back( rightNode );

        leftNode->m_BBox.Reset();
        rightNode->m_BBox.Reset();
        leftNode->m_LeafList.clear();
        rightNode->m_LeafList.clear();

        // Decide wich axis to split
        const unsigned int axis_to_split = aNodeParent->m_BBox.MaxDimension();

        // Divide the objects
        switch( axis_to_split )
        {
            case 0: aNodeParent->m_LeafList.sort( sortByCentroid_X ); break;
            case 1: aNodeParent->m_LeafList.sort( sortByCentroid_Y ); break;
            case 2: aNodeParent->m_LeafList.sort( sortByCentroid_Z ); break;
        }

        unsigned int i = 0;

        for( CONST_LIST_OBJECT2D::const_iterator ii = aNodeParent->m_LeafList.begin();
             ii != aNodeParent->m_LeafList.end();
             ++ii )
        {
            const COBJECT2D *object = static_cast<const COBJECT2D *>(*ii);

            if( i < (aNodeParent->m_LeafList.size() / 2 ) )
            {
                leftNode->m_BBox.Union( object->GetBBox() );
                leftNode->m_LeafList.push_back( object );
            }
            else
            {
                rightNode->m_BBox.Union( object->GetBBox() );
                rightNode->m_LeafList.push_back( object );
            }

            i++;
        }

        wxASSERT( leftNode->m_LeafList.size() > 0 );
        wxASSERT( rightNode->m_LeafList.size() > 0 );
        wxASSERT( ( leftNode->m_LeafList.size() + rightNode->m_LeafList.size() ) ==
                  aNodeParent->m_LeafList.size() );

        aNodeParent->m_Children[0] = leftNode;
        aNodeParent->m_Children[1] = rightNode;
        aNodeParent->m_LeafList.clear();

        recursiveBuild_MIDDLE_SPLIT( leftNode );
        recursiveBuild_MIDDLE_SPLIT( rightNode );

        wxASSERT( aNodeParent->m_LeafList.size() == 0 );
    }
    else
    {
        // It is a Leaf
        aNodeParent->m_Children[0] = NULL;
        aNodeParent->m_Children[1] = NULL;
    }

    wxASSERT( aNodeParent != NULL );
    wxASSERT( aNodeParent->m_BBox.IsInitialized() == true );
}


bool CBVHCONTAINER2D::IntersectAny( const RAYSEG2D &aSegRay ) const
{
    wxASSERT( m_isInitialized == true );

    if( m_Tree )
        return recursiveIntersectAny( m_Tree, aSegRay );

    return false;
}


bool CBVHCONTAINER2D::recursiveIntersectAny( const BVH_CONTAINER_NODE_2D *aNode,
                                             const RAYSEG2D &aSegRay ) const
{
    wxASSERT( aNode != NULL );

    if( aNode->m_BBox.Inside( aSegRay.m_Start ) ||
        aNode->m_BBox.Inside( aSegRay.m_End ) ||
        aNode->m_BBox.Intersect( aSegRay ) )
    {
        if( !aNode->m_LeafList.empty() )
        {
            wxASSERT( aNode->m_Children[0] == NULL );
            wxASSERT( aNode->m_Children[1] == NULL );

            // Leaf
            for( const COBJECT2D *obj : aNode->m_LeafList )
            {
                if( obj->IsPointInside( aSegRay.m_Start ) ||
                    obj->IsPointInside( aSegRay.m_End ) ||
                    obj->Intersect( aSegRay, nullptr, nullptr ) )
                    return true;
            }
        }
        else
        {
            wxASSERT( aNode->m_Children[0] != NULL );
            wxASSERT( aNode->m_Children[1] != NULL );

            // Node
            if( recursiveIntersectAny( aNode->m_Children[0], aSegRay ) )
                return true;
            if( recursiveIntersectAny( aNode->m_Children[1], aSegRay ) )
                return true;
        }
    }

    return false;
}

void CBVHCONTAINER2D::GetListObjectsIntersects( const CBBOX2D &aBBox,
                                                CONST_LIST_OBJECT2D &aOutList ) const
{
    wxASSERT( aBBox.IsInitialized() == true );
    wxASSERT( m_isInitialized == true );

    aOutList.clear();

    if( m_Tree )
        recursiveGetListObjectsIntersects( m_Tree, aBBox, aOutList );
}


void CBVHCONTAINER2D::recursiveGetListObjectsIntersects( const BVH_CONTAINER_NODE_2D *aNode,
                                                         const CBBOX2D & aBBox,
                                                         CONST_LIST_OBJECT2D &aOutList ) const
{
    wxASSERT( aNode != NULL );
    wxASSERT( aBBox.IsInitialized() == true );

    if( aNode->m_BBox.Intersects( aBBox ) )
    {
        if( !aNode->m_LeafList.empty() )
        {
            wxASSERT( aNode->m_Children[0] == NULL );
            wxASSERT( aNode->m_Children[1] == NULL );

            // Leaf
            for( CONST_LIST_OBJECT2D::const_iterator ii = aNode->m_LeafList.begin();
                 ii != aNode->m_LeafList.end();
                 ++ii )
            {
                const COBJECT2D *obj = static_cast<const COBJECT2D *>(*ii);

                if( obj->Intersects( aBBox ) )
                    aOutList.push_back( obj );
            }
        }
        else
        {
            wxASSERT( aNode->m_Children[0] != NULL );
            wxASSERT( aNode->m_Children[1] != NULL );

            // Node
            recursiveGetListObjectsIntersects( aNode->m_Children[0], aBBox, aOutList );
            recursiveGetListObjectsIntersects( aNode->m_Children[1], aBBox, aOutList );
        }
    }
}