//
//  Motion.m
//  BlockyFroggy
//
//  Created by John Ryland on 19/9/17.
//  Copyright © 2017 John Ryland. All rights reserved.
//

#import <Foundation/Foundation.h>
#import <CoreMotion/CoreMotion.h>
#import <CoreLocation/CoreLocation.h>
#import <CoreLocation/CLLocationManagerDelegate.h>
#include "Motion.h"

@interface LocationDelegate : NSObject <CLLocationManagerDelegate> //<NSObject>
@end

@interface LocationDelegate ()
  @property (strong, nonatomic) CLLocation *location;
@end

@implementation LocationDelegate
  - (void)locationManager:(CLLocationManager *)manager
      didUpdateLocations:(NSArray<CLLocation *> *)locations
  {
    if ([locations count] > 0)
      _location = locations[0];
  }
@end


class MotionTrackingData
{
public:
  LocationDelegate* m_locationDelegate = nullptr;
  CMMotionManager* m_motionManager = nullptr;
  CLLocationManager* m_locationManager = nullptr;
  NSOperationQueue* m_deviceQueue = nullptr;
};


MotionTracking::MotionTracking()
{
  m_motionTrackingData = new MotionTrackingData;
  m_motionTrackingData->m_locationDelegate = [[LocationDelegate alloc] init];
  m_motionTrackingData->m_motionManager = [[CMMotionManager alloc] init];
  m_motionTrackingData->m_locationManager = [[CLLocationManager alloc] init];
  m_motionTrackingData->m_deviceQueue = [[NSOperationQueue alloc] init];
}


MotionTracking::~MotionTracking()
{
  delete m_motionTrackingData;
  //[m_motionManager release];
}


void MotionTracking::startTracking(float a_frequency, std::function<void(double a_translation[3], float a_rotation[4], float a_rotationMatrix[9], float a_acceleration[3])> a_callback)
{
  CLLocationManager* lm = m_motionTrackingData->m_locationManager;
  CMMotionManager* mm = m_motionTrackingData->m_motionManager;

  [lm setDistanceFilter: 500];
  [lm setDesiredAccuracy: kCLLocationAccuracyBest];
  [lm setDelegate: m_motionTrackingData->m_locationDelegate];
  [lm startUpdatingLocation];
  
  
  CLLocation *l = [m_motionTrackingData->m_locationDelegate location];
  l = [[CLLocation alloc] initWithLatitude: 0.0 longitude: 0.0];
  

#if 0
  [mm startDeviceMotionUpdatesUsingReferenceFrame: CMAttitudeReferenceFrameXTrueNorthZVertical];
  mm.accelerometerUpdateInterval = a_frequency;
  [mm startAccelerometerUpdatesToQueue:
      m_motionTrackingData->m_deviceQueue
      withHandler: ^(CMAccelerometerData *d, NSError *error)
  {
     [[NSOperationQueue mainQueue] addOperationWithBlock:^{
       CMAcceleration accel = [d acceleration];
       double translation[3];
       float rotation[4];
       
       translation[0] = accel.x * 0.1;
       translation[1] = accel.y * 0.1;
       translation[2] = accel.z * 0.1;
       
       CMMotionManager* mm = m_motionTrackingData->m_motionManager;
       CMDeviceMotion* dm = [mm deviceMotion];
       CMAttitude *attitude = [dm attitude];
       CMQuaternion q = attitude.quaternion;
       CMRotationMatrix rot = attitude.rotationMatrix;
       /*
       translation[0] = accel.x*rot.m11 + accel.y*rot.m12 + accel.z*rot.m13;
       translation[1] = accel.x*rot.m21 + accel.y*rot.m22 + accel.z*rot.m23;
       translation[2] = accel.x*rot.m31 + accel.y*rot.m32 + accel.z*rot.m33;
       */
       rotation[0] = q.x;
       rotation[1] = q.y;
       rotation[2] = q.z;
       rotation[3] = q.w;
       a_callback(translation, rotation);
     }];
  }];
#else
  //[mm startDeviceMotionUpdates];
  mm.deviceMotionUpdateInterval = a_frequency;
//  [mm startGyroUpdates];
//  [mm startDeviceMotionUpdatesToQueue:
  [mm startDeviceMotionUpdatesUsingReferenceFrame: CMAttitudeReferenceFrameXArbitraryZVertical toQueue:  //CMAttitudeReferenceFrameXTrueNorthZVertical
   m_motionTrackingData->m_deviceQueue
   withHandler: ^(CMDeviceMotion *dm, NSError *error)
   {
   
       [[NSOperationQueue mainQueue] addOperationWithBlock:^{
       CMAcceleration accel = [dm userAcceleration];
       CMAttitude *attitude = [dm attitude];
       CMQuaternion q = attitude.quaternion;
       CMRotationMatrix rot = attitude.rotationMatrix;
       double translation[3];
       float rotation[4];

       /*
       translation[0] = accel.x*rot.m11 + accel.y*rot.m12 + accel.z*rot.m13;
       translation[1] = accel.x*rot.m21 + accel.y*rot.m22 + accel.z*rot.m23;
       translation[2] = accel.x*rot.m31 + accel.y*rot.m32 + accel.z*rot.m33;
       
       translation[0] = accel.x*rot.m11 + accel.y*rot.m21 + accel.z*rot.m31;
       translation[1] = accel.x*rot.m12 + accel.y*rot.m22 + accel.z*rot.m32;
       translation[2] = accel.x*rot.m13 + accel.y*rot.m23 + accel.z*rot.m33;
       */

       translation[0] = accel.x;
       translation[1] = accel.y;
       translation[2] = accel.z;

       rotation[0] = q.x;
       rotation[1] = q.y;
       rotation[2] = q.z;
       rotation[3] = q.w;
       
       float rotM[9];
       rotM[0] = rot.m11; rotM[1] = rot.m12; rotM[2] = rot.m13;
       rotM[3] = rot.m21; rotM[4] = rot.m22; rotM[5] = rot.m23;
       rotM[6] = rot.m31; rotM[7] = rot.m32; rotM[8] = rot.m33;
       float accelV[3];
       accelV[0] = accel.x;
       accelV[1] = accel.y;
       accelV[2] = accel.z;


       CLLocation *l = [m_motionTrackingData->m_locationDelegate location];
       CLLocationCoordinate2D coord = [l coordinate];
       double alt = [l altitude];
       
       translation[0] = coord.longitude;
       translation[1] = coord.latitude;
       translation[2] = alt;
       
       if (coord.longitude != 0.0)
       {
         translation[0] = coord.longitude;
         translation[1] = coord.latitude;
         translation[2] = alt;
       }

       a_callback(translation, rotation, rotM, accelV);
      }];
    }];
#endif
}


void MotionTracking::startTracking(float a_frequency)
{
  CMMotionManager* mm = m_motionTrackingData->m_motionManager;
  mm.deviceMotionUpdateInterval = a_frequency;
  //[mm startDeviceMotionUpdates];
  [mm startGyroUpdates];
  [mm startDeviceMotionUpdatesUsingReferenceFrame: CMAttitudeReferenceFrameXTrueNorthZVertical];
}


void MotionTracking::getTrackingData(float a_translation[3], float a_rotation[4])
{
  CMMotionManager* mm = m_motionTrackingData->m_motionManager;
  CMDeviceMotion* dm = [mm deviceMotion];
  CMAcceleration accel = [dm userAcceleration];
  CMAttitude *attitude = [dm attitude];
  CMQuaternion q = attitude.quaternion;
  CMRotationMatrix rot = attitude.rotationMatrix;

  a_translation[0] = accel.x*rot.m11 + accel.y*rot.m12 + accel.z*rot.m13;
  a_translation[1] = accel.x*rot.m21 + accel.y*rot.m22 + accel.z*rot.m23;
  a_translation[2] = accel.x*rot.m31 + accel.y*rot.m32 + accel.z*rot.m33;
  
  a_rotation[0] = q.x;
  a_rotation[1] = q.y;
  a_rotation[2] = q.z;
  a_rotation[3] = q.w;

  /*
  a_translation[0] = accel.x;
  a_translation[1] = accel.y;
  a_translation[2] = accel.z;
  */
  /*
  a_rotation[2] = attitude.roll;
  a_rotation[1] = attitude.pitch;
  a_rotation[0] = attitude.yaw;
  */
}