/***
 * libccd
 * ---------------------------------
 * Copyright (c)2010,2011 Daniel Fiser <danfis@danfis.cz>
 *
 *
 *  This file is part of libccd.
 *
 *  Distributed under the OSI-approved BSD License (the "License");
 *  see accompanying file BDS-LICENSE for details or see
 *  <http://www.opensource.org/licenses/bsd-license.php>.
 *
 *  This software is distributed WITHOUT ANY WARRANTY; without even the
 *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *  See the License for more information.
 */

#include <stdlib.h>
#include <ccd/ccd.h>
#include <ccdcustom/vec3.h>
#include <ccd/simplex.h>
#include <ccd/dbg.h>

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

/** Finds origin (center) of Minkowski difference (actually it can be any
 *  interior point of Minkowski difference. */
_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
                            ccd_support_t *center);

/** Discovers initial portal - that is tetrahedron that intersects with
 *  origin ray (ray from center of Minkowski diff to (0,0,0).
 *
 *  Returns -1 if already recognized that origin is outside Minkowski
 *  portal.
 *  Returns 1 if origin lies on v1 of simplex (only v0 and v1 are present
 *  in simplex).
 *  Returns 2 if origin lies on v0-v1 segment.
 *  Returns 0 if portal was built.
 */
static int discoverPortal(const void *obj1, const void *obj2,
                          const ccd_t *ccd, ccd_simplex_t *portal);


/** Expands portal towards origin and determine if objects intersect.
 *  Already established portal must be given as argument.
 *  If intersection is found 0 is returned, -1 otherwise */
static int refinePortal(const void *obj1, const void *obj2,
                        const ccd_t *ccd, ccd_simplex_t *portal);

/** Finds penetration info by expanding provided portal. */
static int findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
                       ccd_simplex_t *portal,
                       ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);

/** Finds penetration info if origin lies on portal's v1 */
static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
                            ccd_simplex_t *portal,
                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);

/** Find penetration info if origin lies on portal's segment v0-v1 */
static int findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
                              ccd_simplex_t *portal,
                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);

/** Finds position vector from fully established portal */
static int findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
                    const ccd_simplex_t *portal, ccd_vec3_t *pos);

/** Extends portal with new support point.
 *  Portal must have face v1-v2-v3 arranged to face outside portal. */
_ccd_inline void expandPortal(ccd_simplex_t *portal,
                              const ccd_support_t *v4);

/** Fill dir with direction outside portal. Portal's v1-v2-v3 face must be
 *  arranged in correct order! */
_ccd_inline int portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir);

/** Returns true if portal encapsules origin (0,0,0), dir is direction of
 *  v1-v2-v3 face. */
_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
                                       const ccd_vec3_t *dir);

/** Returns true if portal with new point v4 would reach specified
 *  tolerance (i.e. returns true if portal can _not_ significantly expand
 *  within Minkowski difference).
 *
 *  v4 is candidate for new point in portal, dir is direction in which v4
 *  was obtained. */
_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
                                     const ccd_support_t *v4,
                                     const ccd_vec3_t *dir,
                                     const ccd_t *ccd);

/** Returns true if portal expanded by new point v4 could possibly contain
 *  origin, dir is direction in which v4 was obtained. */
_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
                                         const ccd_support_t *v4,
                                         const ccd_vec3_t *dir);


int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
{
    ccd_simplex_t portal;
    int res;

    // Phase 1: Portal discovery - find portal that intersects with origin
    // ray (ray from center of Minkowski diff to origin of coordinates)
    res = discoverPortal(obj1, obj2, ccd, &portal);
    if (res < 0)
        return 0;
    if (res > 0)
        return 1;

    // Phase 2: Portal refinement
    res = refinePortal(obj1, obj2, ccd, &portal);
    return (res == 0 ? 1 : 0);
}

int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
                      ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
    ccd_simplex_t portal;
    int res;

    // Phase 1: Portal discovery
    res = discoverPortal(obj1, obj2, ccd, &portal);
    if (res < 0){
        // Origin isn't inside portal - no collision.
        return -1;

    }else if (res == 1){
        // Touching contact on portal's v1.
        findPenetrTouch(obj1, obj2, ccd, &portal, depth, dir, pos);

    }else if (res == 2){
        // Origin lies on v0-v1 segment.
        if (findPenetrSegment(obj1, obj2, ccd, &portal, depth, dir, pos) != 0) {
            return -1;
        }

    }else if (res == 0){
        // Phase 2: Portal refinement
        res = refinePortal(obj1, obj2, ccd, &portal);
        if (res < 0) {
            return -1;
        }

        // Phase 3. Penetration info
        if (findPenetr(obj1, obj2, ccd, &portal, depth, dir, pos) != 0) {
            return -1;
        }
    }

    return 0;
}



_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
                            ccd_support_t *center)
{
    ccd->center1(obj1, &center->v1);
    ccd->center2(obj2, &center->v2);
    ccdVec3Sub2(&center->v, &center->v1, &center->v2);
}

static int discoverPortal(const void *obj1, const void *obj2,
                          const ccd_t *ccd, ccd_simplex_t *portal)
{
    ccd_vec3_t dir, va, vb;
    ccd_real_t dot;
    int cont;

    // vertex 0 is center of portal
    findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0));
    ccdSimplexSetSize(portal, 1);

    if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)){
        // Portal's center lies on origin (0,0,0) => we know that objects
        // intersect but we would need to know penetration info.
        // So move center little bit...
        ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO);
        ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va);
    }


    // vertex 1 = support in direction of origin
    ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v);
    ccdVec3Scale(&dir, CCD_REAL(-1.));
    if (ccdVec3SafeNormalize(&dir) != 0) {
        return -1;
    }
    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1));
    ccdSimplexSetSize(portal, 2);

    // test if origin isn't outside of v1
    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir);
    if (ccdIsZero(dot) || dot < CCD_ZERO)
        return -1;


    // vertex 2
    ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v,
                       &ccdSimplexPoint(portal, 1)->v);
    if (ccdIsZero(ccdVec3Len2(&dir))){
        if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)){
            // origin lies on v1
            return 1;
        }else{
            // origin lies on v0-v1 segment
            return 2;
        }
    }

    if (ccdVec3SafeNormalize(&dir) != 0) {
        return -1;
    }
    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2));
    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir);
    if (ccdIsZero(dot) || dot < CCD_ZERO) {
        return -1;
    }

    ccdSimplexSetSize(portal, 3);

    // vertex 3 direction
    ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
                     &ccdSimplexPoint(portal, 0)->v);
    ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
                     &ccdSimplexPoint(portal, 0)->v);
    ccdVec3Cross(&dir, &va, &vb);
    if (ccdVec3SafeNormalize(&dir) != 0) {
        return -1;
    }

    // it is better to form portal faces to be oriented "outside" origin
    dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v);
    if (dot > CCD_ZERO){
        ccdSimplexSwap(portal, 1, 2);
        ccdVec3Scale(&dir, CCD_REAL(-1.));
    }

    while (ccdSimplexSize(portal) < 4){
        __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3));
        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir);
        if (ccdIsZero(dot) || dot < CCD_ZERO) {
            return -1;
        }

        cont = 0;

        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
        // continue
        ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v,
                          &ccdSimplexPoint(portal, 3)->v);
        dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
        if (dot < CCD_ZERO && !ccdIsZero(dot)){
            ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3));
            cont = 1;
        }

        if (!cont){
            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
            // continue
            ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v,
                              &ccdSimplexPoint(portal, 2)->v);
            dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
            if (dot < CCD_ZERO && !ccdIsZero(dot)){
                ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3));
                cont = 1;
            }
        }

        if (cont){
            ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
                             &ccdSimplexPoint(portal, 0)->v);
            ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
                             &ccdSimplexPoint(portal, 0)->v);
            ccdVec3Cross(&dir, &va, &vb);
            if (ccdVec3SafeNormalize(&dir) != 0) {
                return -1;
            }
        }else{
            ccdSimplexSetSize(portal, 4);
        }
    }

    return 0;
}

static int refinePortal(const void *obj1, const void *obj2,
                        const ccd_t *ccd, ccd_simplex_t *portal)
{
    ccd_vec3_t dir;
    ccd_support_t v4;

    while (1){
        // compute direction outside the portal (from v0 throught v1,v2,v3
        // face)
        if (portalDir(portal, &dir) != 0) {
            return -1;
        }

        // test if origin is inside the portal
        if (portalEncapsulesOrigin(portal, &dir))
            return 0;

        // get next support point
        __ccdSupport(obj1, obj2, &dir, ccd, &v4);

        // test if v4 can expand portal to contain origin and if portal
        // expanding doesn't reach given tolerance
        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
                || portalReachTolerance(portal, &v4, &dir, ccd)){
            return -1;
        }

        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
        // difference (direction from v0).
        expandPortal(portal, &v4);
    }

    return -1;
}


static int findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
                       ccd_simplex_t *portal,
                       ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos)
{
    ccd_vec3_t dir;
    ccd_support_t v4;
    unsigned long iterations;

    iterations = 0UL;
    while (1){
        // compute portal direction and obtain next support point
        if (portalDir(portal, &dir) != 0) {
            return -1;
        }

        __ccdSupport(obj1, obj2, &dir, ccd, &v4);

        // reached tolerance -> find penetration info
        if (portalReachTolerance(portal, &v4, &dir, ccd)
                || iterations > ccd->max_iterations){
            *depth = ccdVec3PointTriDist2(ccd_vec3_origin,
                                          &ccdSimplexPoint(portal, 1)->v,
                                          &ccdSimplexPoint(portal, 2)->v,
                                          &ccdSimplexPoint(portal, 3)->v,
                                          pdir);
            *depth = CCD_SQRT(*depth);
            if (ccdVec3SafeNormalize(pdir) != 0) {
                return -1;
            }

            // barycentric coordinates:
            if (findPos(obj1, obj2, ccd, portal, pos) != 0) {
                return -1;
            }

            return 0;
        }

        expandPortal(portal, &v4);

        iterations++;
    }
}

static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
                            ccd_simplex_t *portal,
                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
    // Touching contact on portal's v1 - so depth is zero and direction
    // is unimportant and pos can be guessed
    *depth = CCD_REAL(0.);
    ccdVec3Copy(dir, ccd_vec3_origin);

    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
    ccdVec3Scale(pos, 0.5);
}

static int findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
                              ccd_simplex_t *portal,
                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
    /*
    ccd_vec3_t vec;
    ccd_real_t k;
    */

    // Origin lies on v0-v1 segment.
    // Depth is distance to v1, direction also and position must be
    // computed

    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
    ccdVec3Scale(pos, CCD_REAL(0.5));

    /*
    ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v,
                      &ccdSimplexPoint(portal, 0)->v);
    k  = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v));
    k /= CCD_SQRT(ccdVec3Len2(&vec));
    ccdVec3Scale(&vec, -k);
    ccdVec3Add(pos, &vec);
    */

    ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v);
    *depth = CCD_SQRT(ccdVec3Len2(dir));
    if (ccdVec3SafeNormalize(dir) != 0) {
        return -1;
    }
    return 0;
}


static int findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
                    const ccd_simplex_t *portal, ccd_vec3_t *pos)
{
    ccd_vec3_t dir;
    size_t i;
    ccd_real_t b[4], sum, inv;
    ccd_vec3_t vec, p1, p2;

    if (portalDir(portal, &dir) != 0) {
        return -1;
    }

    // use barycentric coordinates of tetrahedron to find origin
    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
                       &ccdSimplexPoint(portal, 2)->v);
    b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);

    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
                       &ccdSimplexPoint(portal, 2)->v);
    b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);

    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v,
                       &ccdSimplexPoint(portal, 1)->v);
    b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);

    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
                       &ccdSimplexPoint(portal, 1)->v);
    b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);

	sum = b[0] + b[1] + b[2] + b[3];

    if (ccdIsZero(sum) || sum < CCD_ZERO){
		b[0] = CCD_REAL(0.);

        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
                           &ccdSimplexPoint(portal, 3)->v);
        b[1] = ccdVec3Dot(&vec, &dir);
        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
                           &ccdSimplexPoint(portal, 1)->v);
        b[2] = ccdVec3Dot(&vec, &dir);
        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
                           &ccdSimplexPoint(portal, 2)->v);
        b[3] = ccdVec3Dot(&vec, &dir);

		sum = b[1] + b[2] + b[3];
	}

	inv = CCD_REAL(1.) / sum;

    ccdVec3Copy(&p1, ccd_vec3_origin);
    ccdVec3Copy(&p2, ccd_vec3_origin);
    for (i = 0; i < 4; i++){
        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v1);
        ccdVec3Scale(&vec, b[i]);
        ccdVec3Add(&p1, &vec);

        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v2);
        ccdVec3Scale(&vec, b[i]);
        ccdVec3Add(&p2, &vec);
    }
    ccdVec3Scale(&p1, inv);
    ccdVec3Scale(&p2, inv);

    ccdVec3Copy(pos, &p1);
    ccdVec3Add(pos, &p2);
    ccdVec3Scale(pos, 0.5);
    return 0;
}

_ccd_inline void expandPortal(ccd_simplex_t *portal,
                              const ccd_support_t *v4)
{
    ccd_real_t dot;
    ccd_vec3_t v4v0;

    ccdVec3Cross(&v4v0, &v4->v, &ccdSimplexPoint(portal, 0)->v);
    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &v4v0);
    if (dot > CCD_ZERO){
        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &v4v0);
        if (dot > CCD_ZERO){
            ccdSimplexSet(portal, 1, v4);
        }else{
            ccdSimplexSet(portal, 3, v4);
        }
    }else{
        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &v4v0);
        if (dot > CCD_ZERO){
            ccdSimplexSet(portal, 2, v4);
        }else{
            ccdSimplexSet(portal, 1, v4);
        }
    }
}

_ccd_inline int portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir)
{
    ccd_vec3_t v2v1, v3v1;

    ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v,
                       &ccdSimplexPoint(portal, 1)->v);
    ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v,
                       &ccdSimplexPoint(portal, 1)->v);
    ccdVec3Cross(dir, &v2v1, &v3v1);
    if (ccdVec3SafeNormalize(dir) != 0) {
        return -1;
    }
    return 0;
}

_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
                                       const ccd_vec3_t *dir)
{
    ccd_real_t dot;
    dot = ccdVec3Dot(dir, &ccdSimplexPoint(portal, 1)->v);
    return ccdIsZero(dot) || dot > CCD_ZERO;
}

_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
                                     const ccd_support_t *v4,
                                     const ccd_vec3_t *dir,
                                     const ccd_t *ccd)
{
    ccd_real_t dv1, dv2, dv3, dv4;
    ccd_real_t dot1, dot2, dot3;

    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}

    dv1 = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, dir);
    dv2 = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, dir);
    dv3 = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, dir);
    dv4 = ccdVec3Dot(&v4->v, dir);

    dot1 = dv4 - dv1;
    dot2 = dv4 - dv2;
    dot3 = dv4 - dv3;

    dot1 = CCD_FMIN(dot1, dot2);
    dot1 = CCD_FMIN(dot1, dot3);

    return ccdEq(dot1, ccd->mpr_tolerance) || dot1 < ccd->mpr_tolerance;
}

_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
                                         const ccd_support_t *v4,
                                         const ccd_vec3_t *dir)
{
    ccd_real_t dot;
    dot = ccdVec3Dot(&v4->v, dir);
    return ccdIsZero(dot) || dot > CCD_ZERO;
}