#include <math.h>
#include <stdio.h>
#include "cube.h"

const double epsilon = 0.001;

void cube_intersect( double  px, double  py, double  pz,
                     double  vx, double  vy, double  vz,
                     double *ix, double *iy, double *iz )
{
    double botxy = vz;
    double botyz = vx;
    double botzx = vy;
    double d2 = -1.0;
    double topxy = -pz;
    double topyz = -px;
    double topzx = -py;
    double bestt = HUGE_VAL;
/*
    if( px >= 1.0 ) px = 1.0;
    if( py >= 1.0 ) py = 1.0;
    if( pz >= 1.0 ) pz = 1.0;
*/
/*
    while( px > 1.0 || py > 1.0 || pz > 1.0 ) {
        px *= 0.5;
        py *= 0.5;
        pz *= 0.5;
    }
*/

    /* xy plane */
    if( 1 ) { // vz < 0.0 && botxy != 0.0 ) {
        double curt = topxy / botxy;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    /* yz plane */
    if( 1 ) { // vx < -epsilon && botyz != 0.0 ) {
        double curt = topyz / botyz;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    /* zx plane */
    if( 1 ) { // vy < -epsilon && botzx != 0.0 ) {
        double curt = topzx / botzx;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    /* xy plane */
    if( 1 ) { // vz > -epsilon && botxy != 0.0 ) {
        double curt = ( topxy + d2 ) / botxy;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    /* yz plane */
    if( 1 ) { // vx > -epsilon && botyz != 0.0 ) {
        double curt = ( topyz + d2 ) / botyz;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    /* zx plane */
    if( 1 ) { // vy > -epsilon && botzx != 0.0 ) {
        double curt = ( topzx + d2 ) / botzx;
        if( curt >= 0.0 && !isinf( curt ) && !isnan( curt ) && curt < bestt ) {
            double x = ( px + ( vx * curt ) );
            if( x >= -epsilon && x <= 1.0+epsilon ) {
                double y = ( py + ( vy * curt ) );
                if( y >= -epsilon && y <= 1.0+epsilon ) {
                    double z = ( pz + ( vz * curt ) );
                    if( z >= -epsilon && z <= 1.0+epsilon ) {
                        bestt = curt;
                    }
                }
            }
        }
    }

    if( bestt == HUGE_VAL ) {
    //fprintf( stderr, "p %f %f %f, v %f %f %f\n", px, py, pz, vx, vy, vz );
    *ix = 0.0;
    *iy = 0.0;
    *iz = 0.0;
    } else {
    *ix = px + ( vx * bestt );
    *iy = py + ( vy * bestt );
    *iz = pz + ( vz * bestt );
    //fprintf( stderr, "p %f %f %f, v %f %f %f, i %f %f %f\n", px, py, pz, vx, vy, vz, *ix, *iy, *iz );
    }
}