//
//  CATransform3D+Vector.h
//  ViconDataStreamFramework
//
//  Created by Marty Pye on 31/03/14.
//  Copyright (c) 2014 Media Computing Group. All rights reserved.
//

#ifndef ViconDataStreamFramework_CATransform3D_Vector_h
#define ViconDataStreamFramework_CATransform3D_Vector_h

#include <QuartzCore/QuartzCore.h>


/*** Data structures. ***/

struct _CAVector3 {
    CGFloat x, y, z;
};
typedef struct _CAVector3 CAVector3;



/*** Definitions of inline functions. ***/

CA_INLINE CAVector3
CAVector3Make(CGFloat x, CGFloat y, CGFloat z)
{
    CAVector3 p; p.x = x; p.y = y; p.z = z; return p;
}


CA_INLINE CAVector3
CAVector3ApplyAffineTransform(CAVector3 v, CATransform3D t)
{
    CAVector3 result;
    result.x = v.x * t.m11 + v.y * t.m21 + v.z * t.m31 + t.m41;
    result.y = v.x * t.m12 + v.y * t.m22 + v.z * t.m32 + t.m42;
    result.z = v.x * t.m13 + v.y * t.m23 + v.z * t.m33 + t.m43;
    return result;
}

CA_INLINE CAVector3
CAVector3Add(CAVector3 v1, CAVector3 v2)
{
    return CAVector3Make(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
}

CA_INLINE CAVector3
CAVector3Subtract(CAVector3 v1, CAVector3 v2)
{
    return CAVector3Make(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}

CA_INLINE CAVector3
CAVector3Scale(CAVector3 v1, CGFloat scaleFactor)
{
    return CAVector3Make(v1.x * scaleFactor, v1.y * scaleFactor, v1.z * scaleFactor);
}

CA_INLINE CGFloat
CAVector3Dot(CAVector3 v1, CAVector3 v2)
{
    return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
}

CA_INLINE CAVector3
CAVector3Cross(CAVector3 v1, CAVector3 v2)
{
    return CAVector3Make(v1.y * v1.z - v1.z * v1.y,
                         v1.z * v2.x - v1.x * v2.z,
                         v1.x * v2.y - v1.y * v2.x);
}

CA_INLINE CGFloat
CAVector3Magnitude(CAVector3 v)
{
    return sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2));
}

CA_INLINE CAVector3
CAVector3Project(CAVector3 v1, CAVector3 v2)
{
    CGFloat scaleFactor = CAVector3Dot(v2, v1) / pow(CAVector3Magnitude(v2), 2);
    return CAVector3Scale(v2, scaleFactor);
}


CA_INLINE CGFloat
CAVector3Angle(CAVector3 v1, CAVector3 v2)
{
    return acos(CAVector3Dot(v1, v2) / (CAVector3Magnitude(v1) * CAVector3Magnitude(v2)));
}

CA_INLINE NSDictionary *
CADictionaryFromVector3(CAVector3 v1)
{
    return @{@"x": [NSNumber numberWithDouble:v1.x],
             @"y": [NSNumber numberWithDouble:v1.y],
             @"z": [NSNumber numberWithDouble:v1.z]};
}

CA_INLINE CAVector3
CAVector3FromDictionary(NSDictionary *dict)
{
    return CAVector3Make([dict[@"x"] doubleValue],
                         [dict[@"y"] doubleValue],
                         [dict[@"z"] doubleValue]);
}


#endif
