//
//  ObjectPicker.m
//  Deneme
//
//  Created by İbrahim Demir on 11/21/13.
//
//

#import "ObjectPicker.h"

@implementation ObjectPicker

+ (NGLMesh *) getTouchingMesh : (CGPoint) touchingPoint : (NGLCamera *) camera : (NGLGroup3D *) objects
{
    CGPoint touchIn3D;
    touchIn3D.x = (touchingPoint.x / [UIScreen mainScreen].bounds.size.width) * 2.0f - 1.0f;
    touchIn3D.y = (touchingPoint.y / [UIScreen mainScreen].bounds.size.height) * -2.0f + 1.0f;
    NGLmat4 *toWorldNotInversed = [camera matrixViewProjection];
    NGLmat4 *toWorld = calloc(1,sizeof(NGLmat4));

    nglMatrixDescribe(*toWorldNotInversed);

    nglMatrixInverse(*toWorldNotInversed,*toWorld);

    nglMatrixDescribe(*toWorld);

    NGLvec4 from, to;
    from.x = (*toWorld)[0] * touchIn3D.x + (*toWorld)[1] * touchIn3D.y - (*toWorld)[2] + (*toWorld)[3];
    from.y = (*toWorld)[4] * touchIn3D.x + (*toWorld)[5] * touchIn3D.y - (*toWorld)[6] + (*toWorld)[7];
    from.z = (*toWorld)[8] * touchIn3D.x + (*toWorld)[9] * touchIn3D.y - (*toWorld)[10] + (*toWorld)[11];
    from.w = (*toWorld)[12] * touchIn3D.x + (*toWorld)[13] * touchIn3D.y - (*toWorld)[14] + (*toWorld)[15];


    to.x = (*toWorld)[0] * touchIn3D.x + (*toWorld)[1] * touchIn3D.y + (*toWorld)[2] + (*toWorld)[3];
    to.y = (*toWorld)[4] * touchIn3D.x + (*toWorld)[5] * touchIn3D.y + (*toWorld)[6] + (*toWorld)[7];
    to.z = (*toWorld)[8] * touchIn3D.x + (*toWorld)[9] * touchIn3D.y + (*toWorld)[10] + (*toWorld)[11];
    to.w = (*toWorld)[12] * touchIn3D.x + (*toWorld)[13] * touchIn3D.y + (*toWorld)[14] + (*toWorld)[15];

    //NSLog(@"%f %f", touchIn3D.x, touchIn3D.y);
    NSLog(@"from: %f %f %f %f", from.x, from.y, from.z, from.w);
    NSLog(@"to  : %f %f %f %f", to.x, to.y, to.z, to.w);

    int clickedObject = -1;
    float minDist = 99999.0f;

    for(int i=0; i<objects.count; i++){
        float t1;
        NGLvec3 direction = (NGLvec3) [self subVec4: to : from];
        NGLvec3 from3d;
        NGLObject3D *selectedObject = [objects objectWithTag:i];
        from3d.x = from.x - [selectedObject position]->x;
        from3d.y = from.y - [selectedObject position]->y;
        from3d.z = from.z - [selectedObject position]->z;
        float r = [self calculateRadius:selectedObject];
        if([self hasIntersection:from3d :direction : r :t1]){

            if(t1<minDist){
                minDist = t1;
                clickedObject = i;
            }
        }
    }

    //NSLog(@"---%d----", clickedObject);


    NGLMesh * mesh = nil;
    return mesh;


}

+ (float) calculateRadius : (NGLObject3D *) object
{
    float x = object.boundingBox.aligned.max.x-object.boundingBox.aligned.min.x;
    float y = object.boundingBox.aligned.max.y-object.boundingBox.aligned.min.y;
    return sqrt(x*x+y*y)/2;
}

+ (BOOL) hasIntersection : (NGLvec3) from : (NGLvec3) direction : (float) r : (int) t1
{
    float A = [self dotProductself:direction];
    float B = 2.0f * [self dotProduct:from:direction];
    float C = [self dotProductself:from] - r*r;

    float dis = B * B - 4.0f * A * C;

    //NSLog(@"dis: %f",dis);
    if (dis < 0.0f)
        return false;
    float S = sqrt(dis);

    t1 = (-B - S) / (2.0f * A);

    NSLog(@"t1: %d",t1);

    return TRUE;
}

+ (float) dotProductself : (NGLvec3) vec
{
    return vec.x*vec.x+vec.y*vec.y+vec.z*vec.z;
}


+ (float) dotProduct : (NGLvec3) vec1: (NGLvec3) vec2
{
    return vec1.x*vec2.x+vec1.y*vec2.y+vec1.z*vec2.z;
}


+ (NGLvec3) subVec3 : (NGLvec3) vec1 : (NGLvec3) vec2
{
    NGLvec3 result;
    result.x = vec1.x-vec2.x;
    result.y = vec1.y-vec2.y;
    result.z = vec1.z-vec2.z;
    return result;
}


+ (NGLvec3) subVec4 : (NGLvec4) vec1 : (NGLvec4) vec2
{
    NGLvec3 result;
    result.x = vec1.x-vec2.x;
    result.y = vec1.y-vec2.y;
    result.z = vec1.z-vec2.z;
    return result;
}


@end