/*
  This file is part of the Structure SDK.
  Copyright © 2015 Occipital, Inc. All rights reserved.
  http://structure.io
*/

#import "ViewController.h"
#import "ViewController+Camera.h"
#import "ViewController+Sensor.h"
#import "ViewController+OpenGL.h"

#import <Structure/Structure.h>

#include <string>

#include "EncoderConfig.h"
#include "Encoder.h"
#include "uplinksimple_image-codecs.h"

static const std::string BUILD_ID = [[[NSBundle mainBundle] objectForInfoDictionaryKey: (NSString*)kCFBundleVersionKey] UTF8String];
static const std::string APP_VERSION_ID = "Scanner.v1." + BUILD_ID;

static Encoder *g_encoder = nullptr;
static NSTimer *g_timer;

static unsigned g_frameCount = 0;
static unsigned g_encodedFrameCount = 0;
static bool g_running = false;
static bool g_receivedIntrinsics = false;
// TODO: Split up writing logic to a separate class

#ifdef WRITE_LOCAL
#define WRITE fwrite
#else
#define WRITE sendNalu
#endif

static std::string g_dataDir;
static std::string g_filePrefix;
static FILE *g_h264File;
static FILE *g_depthFile;
static FILE *g_metaFile;
static NSLock* g_h264Lock = [NSLock new];
static NSLock* g_depthLock = [NSLock new];


// 4-byte NALU start code
static uint8_t g_startCode[4] = {0, 0, 0, 1};

uint8_t* g_depthCompressed = nullptr;

void writeIntrinsics(const Options& options)
{
    unsigned int colorWidth = options.useHalfResColor ? options.colorWidth / 2 : options.colorWidth;
    unsigned int colorHeight = options.useHalfResColor ? options.colorHeight / 2 : options.colorHeight;
    fprintf(g_metaFile, "colorWidth = %d\r\n", colorWidth);
    fprintf(g_metaFile, "colorHeight = %d\r\n", colorHeight);
    fprintf(g_metaFile, "depthWidth = %d\r\n", options.depthWidth);
    fprintf(g_metaFile, "depthHeight = %d\r\n", options.depthHeight);
    
    fprintf(g_metaFile, "fx_color = %f\r\n", options.colorFocalX);
    fprintf(g_metaFile, "fy_color = %f\r\n", options.colorFocalY);
    fprintf(g_metaFile, "mx_color = %f\r\n", options.colorCenterX);
    fprintf(g_metaFile, "my_color = %f\r\n", options.colorCenterY);
    
    fprintf(g_metaFile, "fx_depth = %f\r\n", options.depthFocalX);
    fprintf(g_metaFile, "fy_depth = %f\r\n", options.depthFocalY);
    fprintf(g_metaFile, "mx_depth = %f\r\n", options.depthCenterX);
    fprintf(g_metaFile, "my_depth = %f\r\n", options.depthCenterY);
    
    std::string colorToDepthExt = "";
    for(int i = 0; i < 16; i++) {
        colorToDepthExt += std::to_string(options.colorToDepthExtrinsics[i]) + " ";
    }
    
    fprintf(g_metaFile, "colorToDepthExtrinsics = %s\r\n", colorToDepthExt.c_str());
    
    fprintf(g_metaFile, "deviceId = %s\r\n", options.deviceId.c_str());
    fprintf(g_metaFile, "deviceName = %s\r\n", options.deviceName.c_str());
    fprintf(g_metaFile, "sceneLabel = %s\r\n", options.sceneLabel.c_str());
    fprintf(g_metaFile, "sceneType = %s\r\n", options.specifiedSceneType.c_str());
    fprintf(g_metaFile, "userName = %s\r\n", options.userName.c_str());
    fprintf(g_metaFile, "appVersionId = %s\r\n", APP_VERSION_ID.c_str());
    fflush(g_metaFile);
}

void writeDepthFrame(STDepthFrame* depthFrame)
{
    uint32_t size = uplinksimple::encode([depthFrame shiftData],
                                         [depthFrame height] * [depthFrame width],
                                         g_depthCompressed, sizeof(uint8_t)*[depthFrame height] * (2*[depthFrame width]+1));
    [g_depthLock lock];
    WRITE((uint8_t *) &size, sizeof(uint32_t), 1, g_depthFile);
    WRITE(g_depthCompressed, sizeof(uint8_t), size, g_depthFile); //size is #bytes
    [g_depthLock unlock];
}

void sendNalu(const void *src, size_t chunkSize, size_t numChunks, FILE *unused)
{
    static int i = 0;
    // Assumes chunkSize is always 1 (byte)
    // Does not use FILE*. Included for fwrite compatibility
//    [g_client sendNalU:(UInt8 *)src length:numChunks];
    fwrite(src, chunkSize, numChunks, unused);
    NSLog(@"Sent %d %zu chunks", i++, numChunks);
}

void writeToFile(CMSampleBufferRef sampleBuffer)
{
    if (!CMSampleBufferDataIsReady(sampleBuffer))
    {
        NSLog(@"sampleBuffer is not ready ");
        return;
    }
    
    bool keyFrame = !CFDictionaryContainsKey((CFDictionaryRef) CFArrayGetValueAtIndex(CMSampleBufferGetSampleAttachmentsArray(sampleBuffer, true), 0), kCMSampleAttachmentKey_NotSync);

    // SPS, PPS are their own NALUs
    if (keyFrame)
    {
        CMFormatDescriptionRef format = CMSampleBufferGetFormatDescription(sampleBuffer);

        size_t sparameterSetSize, sparameterSetCount;
        const uint8_t *sparameterSet;
        OSStatus statusCode = CMVideoFormatDescriptionGetH264ParameterSetAtIndex(format, 0, &sparameterSet, &sparameterSetSize, &sparameterSetCount, 0 );
        if (statusCode == noErr)
        {
            // Found sps and now check for pps
            size_t pparameterSetSize, pparameterSetCount;
            const uint8_t *pparameterSet;
            OSStatus statusCode = CMVideoFormatDescriptionGetH264ParameterSetAtIndex(format, 1, &pparameterSet, &pparameterSetSize, &pparameterSetCount, 0 );
            if (statusCode == noErr)
            {
                WRITE(g_startCode, 1, 4, g_h264File);
                WRITE(sparameterSet, 1, sparameterSetSize, g_h264File);
                WRITE(g_startCode, 1, 4, g_h264File);
                WRITE(pparameterSet, 1, pparameterSetSize, g_h264File);
            }
        }
    }
    
    CMBlockBufferRef buf = CMSampleBufferGetDataBuffer(sampleBuffer);
    
    size_t length, totalLength;
    char *dataPointer;

    OSStatus statusCodeRet = CMBlockBufferGetDataPointer(buf, 0, &length, &totalLength, &dataPointer);
    
    if (statusCodeRet == noErr)
    {
        size_t bufferOffset = 0;
        static const int AVCCHeaderLength = 4;
        unsigned counter = 0;
        // Iterate through the NALUs
        while (bufferOffset < totalLength - AVCCHeaderLength) {
            // Read the NAL unit length
            uint32_t NALUnitLength = 0;
            memcpy(&NALUnitLength, dataPointer + bufferOffset, AVCCHeaderLength);
            
            // Convert the length value from Big-endian to Little-endian
            NALUnitLength = CFSwapInt32BigToHost(NALUnitLength);

            WRITE(g_startCode, 1, 4, g_h264File);
            WRITE(dataPointer + bufferOffset + AVCCHeaderLength, 1, NALUnitLength, g_h264File);
            
            bufferOffset += AVCCHeaderLength + NALUnitLength;
        }
    }
}

void h264EncodedFrameCallback(CMSampleBufferRef sampleBuffer, void *userData)
{
    if (g_running) {
        [g_h264Lock lock];
        writeToFile(sampleBuffer);
        [g_h264Lock unlock];
        
        dispatch_async(dispatch_get_main_queue(), ^{
            g_encodedFrameCount++;
        });
    }
}

@implementation ViewController (Sensor)

#pragma mark -  Structure Sensor delegates

- (void)setupStructureSensor
{
    // Get the sensor controller singleton
    _sensorController = [STSensorController sharedController];
    
    // Set ourself as the delegate to receive sensor data.
    _sensorController.delegate = self;
    
    NSArray *paths = NSSearchPathForDirectoriesInDomains
    (NSDocumentDirectory, NSUserDomainMask, YES);
    NSString *documentsDirectory = [paths objectAtIndex:0];
    documentsDirectory = [documentsDirectory stringByAppendingString:@"/"];
    
    g_dataDir = [documentsDirectory UTF8String];
}

- (BOOL)isStructureConnectedAndCharged
{
    return [_sensorController isConnected] && ![_sensorController isLowPower];
}

- (void)sensorDidConnect
{
    NSLog(@"[Structure] Sensor connected!");
    
    if (![self currentStateNeedsSensor])
    {
        [self enterSceneLabellingState];
        //[self enterCubePlacementState];
    }
    [self connectToStructureSensorAndStartStreaming];
}

- (void)sensorDidLeaveLowPowerMode
{
    _appStatus.sensorStatus = AppStatus::SensorStatusNeedsUserToConnect;
    [self updateAppStatusMessage];
}

- (void)sensorBatteryNeedsCharging
{
    [self cleanUpAndReset];
    // Notify the user that the sensor needs to be charged.
    _appStatus.sensorStatus = AppStatus::SensorStatusNeedsUserToCharge;
    [self updateAppStatusMessage];
}

- (void)cleanUpFileWriting
{
    if (g_encoder) { delete g_encoder; g_encoder = nullptr; }
    if (g_depthCompressed) { delete [] g_depthCompressed; g_depthCompressed = nullptr; }
}

- (void)sensorDidStopStreaming:(STSensorControllerDidStopStreamingReason)reason
{
    if (reason == STSensorControllerDidStopStreamingReasonAppWillResignActive)
    {
        [self stopColorCamera];
        NSLog(@"[Structure] Stopped streaming because the app will resign its active state.");
    }
    else
    {
        NSLog(@"[Structure] Stopped streaming for an unknown reason.");
        [self enterViewingState];
    }
}

- (void)sensorDidDisconnect
{
    // If we receive the message while in background, do nothing. We'll check the status when we
    // become active again.
    if ([[UIApplication sharedApplication] applicationState] != UIApplicationStateActive)
        return;
    
    NSLog(@"[Structure] Sensor disconnected!");
    
    // Reset the scan on disconnect, since we won't be able to recover afterwards.
    if (_slamState.scannerState == ScannerStateScanning)
    {
        [self resetButtonPressed:self];
        [self enterViewingState];
    }
    
    if (_useColorCamera)
        [self stopColorCamera];
    
    [self updateIdleTimer];
}


- (STSensorControllerInitStatus)connectToStructureSensorAndStartStreaming
{
    
    // Try connecting to a Structure Sensor.
    STSensorControllerInitStatus result = [_sensorController initializeSensorConnection];
    
    if (result == STSensorControllerInitStatusSuccess || result == STSensorControllerInitStatusAlreadyInitialized)
    {
        // Even though _useColorCamera was set in viewDidLoad by asking if an approximate calibration is guaranteed,
        // it's still possible that the Structure Sensor that has just been plugged in has a custom or approximate calibration
        // that we couldn't have known about in advance.
        
        STCalibrationType calibrationType = [_sensorController calibrationType];
        if(calibrationType == STCalibrationTypeApproximate || calibrationType == STCalibrationTypeDeviceSpecific)
        {
            _useColorCamera = true;
        }
        else
        {
            _useColorCamera = false;
        }

        // If we can't use the color camera, then don't try to use registered depth.
        if (!_useColorCamera)
            _options.useHardwareRegisteredDepth = false;
        
        _appStatus.sensorStatus = AppStatus::SensorStatusOk;
        [self updateAppStatusMessage];
        
        // Start streaming depth data.
        [self startStructureSensorStreaming];
    }
    else
    {
        switch (result)
        {
            case STSensorControllerInitStatusSensorNotFound:
                NSLog(@"[Structure] No sensor found"); break;
            case STSensorControllerInitStatusOpenFailed:
                NSLog(@"[Structure] Error: Open failed."); break;
            case STSensorControllerInitStatusSensorIsWakingUp:
                NSLog(@"[Structure] Error: Sensor still waking up."); break;
            default: {}
        }
        
        _appStatus.sensorStatus = AppStatus::SensorStatusNeedsUserToConnect;
        [self updateAppStatusMessage];
    }
    
    [self updateIdleTimer];
    
    return result;
}

- (void)startStructureSensorStreaming
{
    if (![self isStructureConnectedAndCharged])
        return;
    
    // Tell the driver to start streaming.
    NSError *error = nil;
    BOOL optionsAreValid = FALSE;
    if (_useColorCamera)
    {
        // We can use either registered or unregistered depth.
        _structureStreamConfig = _options.useHardwareRegisteredDepth ? STStreamConfigRegisteredDepth640x480 : STStreamConfigDepth640x480;
        
        if (_options.useHardwareRegisteredDepth)
        {
            // We are using the color camera, so let's make sure the depth gets synchronized with it.
            // If we use registered depth, we also need to specify a fixed lens position value for the color camera.
            optionsAreValid = [_sensorController startStreamingWithOptions:@{kSTStreamConfigKey : @(_structureStreamConfig),
                                                                             kSTFrameSyncConfigKey : @(STFrameSyncDepthAndRgb),
                                                                             kSTColorCameraFixedLensPositionKey: @(_options.lensPosition)}
                                                                     error:&error];
        }
        else
        {
            // We are using the color camera, so let's make sure the depth gets synchronized with it.
            optionsAreValid = [_sensorController startStreamingWithOptions:@{kSTStreamConfigKey : @(_structureStreamConfig),
                                                                             kSTFrameSyncConfigKey : @(STFrameSyncDepthAndRgb)}
                                                                     error:&error];
        }
        
        [self startColorCamera];
    }
    else
    {
        _structureStreamConfig = STStreamConfigDepth640x480;
        
        optionsAreValid = [_sensorController startStreamingWithOptions:@{kSTStreamConfigKey : @(_structureStreamConfig),
                                                                         kSTFrameSyncConfigKey : @(STFrameSyncOff)} error:&error];
    }
    
    if (!optionsAreValid)
    {
        NSLog(@"Error during streaming start: %s", [[error localizedDescription] UTF8String]);
        return;
    }
    
    NSLog(@"[Structure] Streaming started.");
    
    // Notify and initialize streaming dependent objects.
    [self onStructureSensorStartedStreaming];
}

- (void)onStructureSensorStartedStreaming
{
    NSLog(@"onStructureSensorStartedStreaming");
    
    if (!_slamState.initialized)
    {
        [self enterSceneLabellingState];

        _depthAsRgbaVisualizer = [[STDepthToRgba alloc] initWithOptions:@{kSTDepthToRgbaStrategyKey: @(STDepthToRgbaStrategyRedToBlueGradient)}
                                                                  error:nil];

        _slamState.initialized = true;
    }
}

- (void)sensorDidOutputDeviceMotion:(CMDeviceMotion*)motion
{
    //[self processDeviceMotion:motion withError:nil];
}

GLKVector4 getIntrinsicsFromGlProj(const GLKMatrix4& matrix, unsigned int width, unsigned int height, bool useHalf)
{
    float fov = 2.0f * atan(1.0f / matrix.m00);
    float aspect = (float)width / height;
    float t = tan(0.5f * fov);
    float fx = 0.5f * width / t;
    float fy = 0.5f * height / t * aspect;
    
    float mx = (float)(width - 1.0f) / 2.0f;
    float my = (float)(height - 1.0f) / 2.0f;
 
    if (useHalf) {
        fx *= 0.5f; fy *= 0.5f;
        mx *= 0.5f; my *= 0.5f;
    }
    GLKVector4 ret = GLKVector4Make(fx, fy, mx, my);
    return ret;
}

-(UIImage *) imageFromFrame:(STColorFrame*) colorFrame{
    CMSampleBufferRef sampleBuffer = colorFrame.sampleBuffer;
    @autoreleasepool {
        CVImageBufferRef imBuffer = CMSampleBufferGetImageBuffer(sampleBuffer);
        CVPixelBufferLockBaseAddress(imBuffer, 0);
        
        CGColorSpaceRef colorSpace = CGColorSpaceCreateDeviceGray();
        
        CGContextRef context = CGBitmapContextCreate(CVPixelBufferGetBaseAddressOfPlane(imBuffer, 0), CVPixelBufferGetWidth(imBuffer), CVPixelBufferGetHeight(imBuffer), 8, CVPixelBufferGetBytesPerRowOfPlane(imBuffer,0), colorSpace, kCGImageAlphaNone);
        
        CGImageRef cgImage = CGBitmapContextCreateImage(context);
        CVPixelBufferUnlockBaseAddress(imBuffer,0);
        
        CGContextRelease(context);
        CGColorSpaceRelease(colorSpace);
        
        UIImage *image = [UIImage imageWithCGImage:cgImage];
        
        CGImageRelease(cgImage);
        
        return image;
    }
}

- (bool)sensorTooClose:(STDepthFrame*)depthFrame
{
    float *depth = depthFrame.depthInMillimeters;
    int totalSize = depthFrame.width * depthFrame.height;
    int numNotNan = 0;
    int numValidDistance = 0;
    
    const double distanceThreshold = 750.0;
    const double threshold = 0.45;
    
    for(int i = 0; i < totalSize; i++)
    {
        if(!isnan(depth[i]))
        {
            numNotNan++;
            if(depth[i] < distanceThreshold)
            {
                numValidDistance++;
            }
        }
    }
    
    double percentGood = ((double) numValidDistance) / numNotNan;
    return (percentGood > threshold) || isnan(percentGood);
    
}

-(void)numFeatures:(UIImage*)inputImage
{
    //NSLog(@"Beginning corner detection");
    GPUImagePicture *pictureInput = [[GPUImagePicture alloc] initWithImage:inputImage];
    GPUImageHarrisCornerDetectionFilter *cornerDetector = [[GPUImageHarrisCornerDetectionFilter alloc] init];
    [cornerDetector forceProcessingAtSize:inputImage.size];
    [cornerDetector setThreshold:0.12];
    [cornerDetector setSensitivity:25.0];
    [pictureInput removeAllTargets];
    
    [cornerDetector setCornersDetectedBlock:^(GLfloat* cornerArray, NSUInteger cornersDetected, CMTime frameTime) {
       
        //NSLog(@"Number of corners: %ld", (unsigned long)cornersDetected);

        /*dispatch_async(dispatch_get_main_queue(), ^{
            if(g_frameCount % 5 == 0)
            {
                bool hasNoCorners = cornersDetected < numCornerThreshold;
                [[self losingTrackingLabel] setHidden:!(hasNoCorners && prevFrameNoFeatures && prevFrame2NoFeatures)];
                
                prevFrame2NoFeatures = prevFrameNoFeatures;
                prevFrameNoFeatures = hasNoCorners;
            }
        });*/
        const unsigned int numCornerThreshold = 3;
        
        for(int i = 0; i < cornersDetected; i++)
        {
            [corners addObject:[NSNumber numberWithFloat:cornerArray[2 * i]]];
            [corners addObject:[NSNumber numberWithFloat:cornerArray[2 * i + 1]]];
        }
        

        bool hasNoCorners = cornersDetected < numCornerThreshold;
        const float maxCorners = 25;
        dispatch_async(dispatch_get_main_queue(), ^{
            int numCornersForColor = (int) MIN(cornersDetected, maxCorners);
            for(int i = 0; i < numPrevFramesTracked - 1; i++) {
                numPreviousCorners[i] = numPreviousCorners[i + 1];
            }
            numPreviousCorners[numPrevFramesTracked - 1] = numCornersForColor;
            
            numCornersForColor = *std::max_element(numPreviousCorners, numPreviousCorners + numPrevFramesTracked);
            
            float progress = 0;
            if(numCornersForColor <= 2) {
                progress = 0.2;
            }
            else if(numCornersForColor >= maxCorners) {
                progress = 1.0;
            }
            else {
                progress = 0.2 + log2(numCornersForColor) / (log2(maxCorners) * 1.25);
            }
            
            /*
            CGFloat redColor   = CGFloat( ( (float) (maxCorners - numCornersForColor)) / maxCorners );
            CGFloat greenColor = CGFloat( ( (float) (numCornersForColor) / maxCorners));
            UIColor *color = [[UIColor alloc] initWithRed:redColor green:greenColor blue:0 alpha:1];
             */
            
            float hue = 1.0f / 3 * log2(numCornersForColor) / log2(maxCorners);
            //NSLog(@"Hue: %f", hue);
            UIColor *color = [[UIColor alloc] initWithHue:hue saturation:1 brightness:1 alpha:1];
            /*
            if(cornersDetected <= 3) {
                color = [UIColor redColor];
                //progress = 0.4;
            }
            else if(cornersDetected < 6) {
                //progress = 0.5;
                color = [UIColor orangeColor];
            }
            else {
                //progress = 1.0;
                color = [UIColor greenColor];
            }*/
            
            //[[self losingTrackingLabel] setHidden:!hasNoCorners];
            [[self numCornersProgressView] setProgressTintColor:color];
            [[self numCornersProgressView] setProgress:progress animated:YES];
        });

       // });

    }];
    
    [pictureInput addTarget:cornerDetector];
    [cornerDetector useNextFrameForImageCapture];
    [pictureInput processImage];
    //NSLog(@"XCorners: %lu", [corners count]);
}

- (void)processDepthFrame:(STDepthFrame *)depthFrame
          colorFrameOrNil:(STColorFrame*)colorFrame
{
    // Upload the new color image for next rendering.
    if (_useColorCamera && colorFrame != nil)
    {
        [self uploadGLColorTexture: colorFrame];
    }
    //else if(!_useColorCamera)
    //{
    [self uploadGLColorTextureFromDepth:depthFrame];
    //}

    // Update the projection matrices since we updated the frames.
    {
        _display.depthCameraGLProjectionMatrix = [depthFrame glProjectionMatrix];
        if (colorFrame)
            _display.colorCameraGLProjectionMatrix = [colorFrame glProjectionMatrix];
    }

}

- (void)sensorDidOutputSynchronizedDepthFrame:(STDepthFrame*)depthFrame
                                andColorFrame:(STColorFrame*)colorFrame
{
    
    if (_slamState.initialized)
    {
        //NSLog(@"process depth/color frame");
        [self processDepthFrame:depthFrame colorFrameOrNil:colorFrame];
        if ((_slamState.scannerState == ScannerStateSceneLabelling || _slamState.scannerState == ScannerStateCubePlacement) && colorFrame && depthFrame) {
            GLKVector4 colorIntrinsics = getIntrinsicsFromGlProj(colorFrame.glProjectionMatrix, _options.colorWidth, _options.colorHeight, _options.useHalfResColor);
            GLKVector4 depthIntrinsics = getIntrinsicsFromGlProj(depthFrame.glProjectionMatrix, _options.depthWidth, _options.depthHeight, false);
            //NSLog(@"color intrinsics: %f %f %f %f (%d,%d)\n", colorIntrinsics.x, colorIntrinsics.y, colorIntrinsics.z, colorIntrinsics.w, _options.colorWidth, _options.colorHeight);
            //NSLog(@"depth intrinsics: %f %f %f %f (%d,%d)\n", depthIntrinsics.x, depthIntrinsics.y, depthIntrinsics.z, depthIntrinsics.w, _options.depthWidth, _options.depthHeight);
            _options.colorFocalX = colorIntrinsics.x;
            _options.colorFocalY = colorIntrinsics.y;
            _options.colorCenterX = colorIntrinsics.z;
            _options.colorCenterY = colorIntrinsics.w;
            
            _options.depthFocalX = depthIntrinsics.x;
            _options.depthFocalY = depthIntrinsics.y;
            _options.depthCenterX = depthIntrinsics.z;
            _options.depthCenterY = depthIntrinsics.w;
            
            /*
            float *m4 = (float*) malloc(16 * sizeof(float));
            [depthFrame colorCameraPoseInDepthCoordinateFrame:m4];
            
            for(int i = 0; i < 4; i++) {
                NSLog(@"%f %f %f %f", m4[i], m4[i + 4], m4[i + 8], m4[i + 12]);
            }
            free(m4);
            */
            
            if(!_options.useHardwareRegisteredDepth) {
                float *m4 = (float*) malloc(16 * sizeof(float));
                [_sensorController colorCameraPoseInSensorCoordinateFrame:m4];
            
                for(int i = 0; i < 4; i++) {
                    for(int j = 0; j < 4; j++) {
                        _options.colorToDepthExtrinsics[4 * i + j] = m4[i + 4 * j];
                    }
                    //NSLog(@"%f %f %f %f", m4[i], m4[i + 4], m4[i + 8], m4[i + 12]);
                }
                free(m4);
            }
            else {
                for(int i = 0; i < 4; i++) {
                    for(int j = 0; j < 4; j++) {
                        _options.colorToDepthExtrinsics[4 * i + j] = (i == j);
                    }
                }
            }
            g_receivedIntrinsics = true;
        }
        if (_slamState.scannerState == ScannerStateScanning && g_running && colorFrame && depthFrame)
        {
            //if(g_frameCount % 100 == 0)
              //  NSLog(@"Frame Count: %d", g_frameCount);
            
            colorFrame = colorFrame.halfResolutionColorFrame;
            // Save first frame out as image
            if(g_frameCount == 1)
            {
                UIImage *thumbnail = [self imageFromFrame:colorFrame];
                
                std::string curFilePrefix = g_dataDir + g_filePrefix;
                NSString *filePath = [NSString stringWithUTF8String:(curFilePrefix + ".jpg").c_str()];
                //NSLog(filePath);
                [UIImageJPEGRepresentation(thumbnail, 1) writeToFile: filePath atomically:YES];
            }
            
            g_encoder->encodeFrame(colorFrame);
            writeDepthFrame(depthFrame);
            _colorTimestamps.push_back([colorFrame timestamp]);
            _depthTimestamps.push_back([depthFrame timestamp]);
            
//#ifndef DEBUG_MODE
            //if((g_frameCount % 5) == 0) // Check if too close or losing tracking every 5 frames
            //{
            /*
            if((g_frameCount % 10) == 0)
                [[self tooCloseLabel] setHidden:(![self sensorTooClose:depthFrame])];
            */
            UIImage* im = (colorFrame.height > 500) ? [self imageFromFrame:colorFrame.halfResolutionColorFrame] : [self imageFromFrame:colorFrame];

            //UIImage* im = [self imageFromFrame:colorFrame];
            [self numFeatures:im];
            
            NSLog(@"Exposure Duration: %f, ISO: %f", CMTimeGetSeconds([self.videoDevice exposureDuration]), self.videoDevice.ISO);
            
            AVCaptureWhiteBalanceGains wb = [self.videoDevice deviceWhiteBalanceGains];
            float redGain = wb.redGain;
            float greenGain = wb.greenGain;
            float blueGain = wb.blueGain;

            float exposureDuration = CMTimeGetSeconds([self.videoDevice exposureDuration]);
            float ISO = self.videoDevice.ISO;
            
            
            //NSLog(@"Blue Gain: %f Green Gain: %f Red Gain: %f", wb.blueGain, wb.greenGain, wb.redGain);
            fwrite((float*) &redGain, sizeof(float), 1, g_cameraFile);
            fwrite((float*) &greenGain, sizeof(float), 1, g_cameraFile);
            fwrite((float*) &blueGain, sizeof(float), 1, g_cameraFile);
            fwrite((float*) &exposureDuration, sizeof(float), 1, g_cameraFile);
            fwrite((float*) &ISO, sizeof(float), 1, g_cameraFile);
            fflush(g_cameraFile);
            //#endif

            dispatch_async(dispatch_get_main_queue(), ^{
                g_frameCount++;
            });
        }
        
        // Scene rendering is triggered by new frames to avoid rendering the same view several times.
        [self renderSceneForDepthFrame:depthFrame colorFrameOrNil:colorFrame];
    }
}

- (void)stopScanningAndWrite
{
    if (g_running) {
        g_running = false;
        [self closeFile];
        [self cleanUpFileWriting];
        g_frameCount = 0;
        g_encodedFrameCount = 0;
        _colorTimestamps.clear();
        _depthTimestamps.clear();
    }
}

- (void)startScanningAndOpen
{
    if (!g_running) {
        unsigned int colorWidth = _options.useHalfResColor ? _options.colorWidth / 2 : _options.colorWidth;
        unsigned int colorHeight = _options.useHalfResColor ? _options.colorHeight / 2 : _options.colorHeight;
        
        NSLog(@"startScanningAndOpen (%d,%d, bitrate %d)", colorWidth, colorHeight, _options.colorEncodeBitrate);
        
        EncoderConfig config(colorWidth, colorHeight, EncoderConfig::H264, 30, _options.colorEncodeBitrate); //30fps
        g_encoder = new Encoder(config);
        g_encoder->registerEncodedFrameCallback(h264EncodedFrameCallback, 0);
        if (!g_depthCompressed) g_depthCompressed = new uint8_t[(2*_options.depthWidth+1) * _options.depthHeight];
        
        NSDateFormatter *dateFormatter=[[NSDateFormatter alloc] init];
        [dateFormatter setDateFormat:@"yyyy-MM-dd_hh-mm-ss"];
        std::string dateString = [[dateFormatter stringFromDate:[NSDate date]] UTF8String];

        if (!g_receivedIntrinsics || _options.deviceId.empty()) {
            NSLog(@"ERROR: no intrinsics/device id!");
            _appStatus.sensorStatus = AppStatus::SensorStatusNeedsIntrinsics;
            [self updateAppStatusMessage];
            g_running = false;
            return;
        }
        else {
            NSLog(@"color intrinsics: %f %f, %f %f", _options.colorFocalX, _options.colorFocalY, _options.colorCenterX, _options.colorCenterY);
            NSLog(@"depth intrinsics: %f %f, %f %f", _options.depthFocalX, _options.depthFocalY, _options.depthCenterX, _options.depthCenterY);
        }

        [self openFile:(dateString + "__" + _options.deviceId)];
        NSLog(@"[Structure] opened file");
        
        writeIntrinsics(_options);
        g_running = true;
    }
}

- (void)sensorDidOutputDepthFrame:(STDepthFrame *)depthFrame
{
    if (_slamState.initialized)
    {
        [self processDepthFrame:depthFrame colorFrameOrNil:nil];
        // Scene rendering is triggered by new frames to avoid rendering the same view several times.
        [self renderSceneForDepthFrame:depthFrame colorFrameOrNil:nil];
    }
}

- (void) openFile:(std::string) filePref
{
    g_h264File = fopen((g_dataDir + filePref + ".h264").c_str(), "wb");
    g_depthFile = fopen((g_dataDir + filePref + ".depth").c_str(), "wb");
    g_metaFile = fopen((g_dataDir + filePref + ".txt").c_str(), "w");
    g_imuFile = fopen((g_dataDir + filePref + ".imu").c_str(), "wb");
    g_cameraFile = fopen((g_dataDir + filePref + ".camera").c_str(), "wb");
    
    
    std::string versionNum = "v1";
    fwrite(versionNum.c_str(), versionNum.size() * sizeof(char), 1, g_cameraFile);

    g_filePrefix = filePref;
}

- (void) closeFile
{
    if (g_frameCount > _colorTimestamps.size() || g_frameCount > _depthTimestamps.size())
        NSLog(@"ERROR: inconsistent #frames/#timestamps (color, depth, frames) = %lu, %lu, %d\n", _colorTimestamps.size(), _depthTimestamps.size(), g_frameCount);

    NSLog(@"framecount = %d, encodedcount = %d, #imu = %d\n", g_frameCount, g_encodedFrameCount, g_numIMUmeasurements);

    //get created at time
    NSDateFormatter *dateFormatter=[[NSDateFormatter alloc] init];
    [dateFormatter setDateFormat:@"yyyyMMdd'T'hhmmssZZZ"];
    std::string dateString = [[dateFormatter stringFromDate:[NSDate date]] UTF8String];

    //write #frames
    fprintf(g_metaFile, "scanSavedAt = %s\r\n", dateString.c_str());
    fprintf(g_metaFile, "numDepthFrames = %d\r\n", g_frameCount);
    fprintf(g_metaFile, "numColorFrames = %d\r\n", g_encodedFrameCount);
    fprintf(g_metaFile, "numIMUmeasurements = %d\r\n", g_numIMUmeasurements);
    fclose(g_metaFile);
    
    //write timestamps to end of depth file
    [g_depthLock lock];
    WRITE(_depthTimestamps.data(), sizeof(NSTimeInterval), g_frameCount, g_depthFile);
    WRITE(_colorTimestamps.data(), sizeof(NSTimeInterval), g_frameCount, g_depthFile);
    fclose(g_depthFile);
    [g_depthLock unlock];
    
    [g_h264Lock lock];
    fclose(g_h264File);
    [g_h264Lock unlock];
    
    [g_imuLock lock];
    fclose(g_imuFile);
    [g_imuLock unlock];
    
    [g_cameraLock lock];
    fclose(g_cameraFile);
    [g_cameraLock unlock];
    
    [self verifyFiles];
    
    NSLog(@"[Structure] closed file");
}

- (void) verifyFiles
{
    bool remove = false;
    std::string curFilePrefix = g_dataDir + g_filePrefix;
    
    NSError *error = nil;
    NSString *absH264File = [NSString stringWithUTF8String:(curFilePrefix + ".h264").c_str()];
    NSString *h264FileSize = [NSString stringWithFormat:@"%llu",
                              [[[NSFileManager defaultManager] attributesOfItemAtPath:absH264File error:&error] fileSize]];
    if (!h264FileSize || error)
    {
        NSLog(@"H264 file %@, size %@, error: %@", absH264File, h264FileSize, error);
        remove = true;
    }
    
    error = nil;
    NSString *absDepthFile = [NSString stringWithUTF8String:(curFilePrefix + ".h264").c_str()];
    NSString *depthFileSize = [NSString stringWithFormat:@"%llu",
                              [[[NSFileManager defaultManager] attributesOfItemAtPath:absDepthFile error:&error] fileSize]];
    if (!depthFileSize || error)
    {
        NSLog(@"Depth file %@, size %@, error: %@", absDepthFile, depthFileSize, error);
        remove = true;
    }
    
    error = nil;
    NSString *absMetaFile = [NSString stringWithUTF8String:(curFilePrefix + ".h264").c_str()];
    NSString *metaFileSize = [NSString stringWithFormat:@"%llu",
                              [[[NSFileManager defaultManager] attributesOfItemAtPath:absMetaFile error:&error] fileSize]];
    if (!metaFileSize || error)
    {
        NSLog(@"Meta file %@, size %@, error: %@", absMetaFile, metaFileSize, error);
        remove = true;
    }
    
    error = nil;
    NSString *absImuFile = [NSString stringWithUTF8String:(curFilePrefix + ".h264").c_str()];
    NSString *imuFileSize = [NSString stringWithFormat:@"%llu",
                              [[[NSFileManager defaultManager] attributesOfItemAtPath:absImuFile error:&error] fileSize]];
    if (!imuFileSize || error)
    {
        NSLog(@"imu file %@, size %@, error: %@", absImuFile, imuFileSize, error);
        remove = true;
    }
    
    if (remove)
    {
        NSFileManager *fileManager = [NSFileManager defaultManager];
        [fileManager removeItemAtPath:absH264File error:nil];
        [fileManager removeItemAtPath:absDepthFile error:nil];
        [fileManager removeItemAtPath:absMetaFile error:nil];
        [fileManager removeItemAtPath:absImuFile error:nil];
    }
}

@end