//=============================================================================================================================
//
// Copyright (c) 2015-2021 VisionStar Information Technology (Shanghai) Co., Ltd. All Rights Reserved.
// EasyAR is the registered trademark or trademark of VisionStar Information Technology (Shanghai) Co., Ltd in China
// and other countries for the augmented reality technology developed by VisionStar Information Technology (Shanghai) Co., Ltd.
//
//=============================================================================================================================
#import "MyCamera.h"

#import <easyar/vector.oc.h>
#import <easyar/camera.oc.h>
#import <easyar/buffer.oc.h>
#import <easyar/image.oc.h>
#import <easyar/cameraparameters.oc.h>
#import <easyar/frame.oc.h>

#include <math.h>
#include <float.h>
#include <string.h>

#import <Endian.h>
#import <UIKit/UIKit.h>

@interface MyCamera() < AVCaptureVideoDataOutputSampleBufferDelegate >
{
    easyar_CameraParameters *CameraParams;
    bool opened_;
}
@property(nonatomic, weak) AVCaptureDevice *videoDevice;
@property(nonatomic, strong) NSMutableArray<AVCaptureDeviceFormat *> *allSupportedFormats;
@property(nonatomic) CGSize previewSize;
@property(nonatomic, strong) AVCaptureVideoDataOutput *captureOutPut;
@end

static float getRatioError(float x, float x0)
{
    float a = (x / MAX(x0, 1.0f) - 1);
    float b = (x0 / MAX(x, 1.0f) - 1);
    return a * a + b * b;
}

@implementation MyCamera

- (instancetype)init
{
    self = [super init];
    
    if (self) {
        self.captureSession = [[AVCaptureSession alloc] init];
        self.captureOutPut = nil;
        opened_ = false;
    }
    return self;
}

-(void) connect:(easyar_InputFrameSink *)inputFrameSink
{
    self.sink = inputFrameSink;
}

-(BOOL)open
{
    self.videoDevice = [self cameraWithPosition : AVCaptureDevicePositionBack];
    if (!self.videoDevice) {
        return FALSE;
    }
    
    NSError *error;
    AVCaptureDeviceInput *videoInput = [AVCaptureDeviceInput deviceInputWithDevice : self.videoDevice error : &error];
    if (error) {
        return FALSE;
    }
    [self setDeviceInput : videoInput];
    
    if ([self.captureSession canAddInput : videoInput]) {
        [self.captureSession addInput : videoInput];
    } else {
        NSLog(@"Could not add input port to capture session %@", self.captureSession);
    }
    
    [self addVideoDataOutput];
    opened_ = true;
    
    self.allSupportedFormats = [[NSMutableArray alloc] init];

    for (AVCaptureDeviceFormat * format in self.videoDevice.formats) {
        [self.allSupportedFormats addObject:format];
    }
    if (self.allSupportedFormats.count > 0) {
        [self setSize : CGSizeMake(1280, 720)];
    }
    
    return TRUE;
}

-(BOOL)start
{
    if (!self.captureSession) {
        return FALSE;
    }
    [self.captureSession startRunning];
    return TRUE;
}

-(BOOL)stop
{
    if (!self.captureSession) {
        return FALSE;
    }
    [self.captureSession stopRunning];
    return TRUE;
}

- (easyar_CameraParameters *) CameraParams
{
    return CameraParams;
}

-(CGSize)size
{
    return self.previewSize;
}

-(BOOL)setSize:(CGSize)size
{
    if (!opened_) {
        return FALSE;
    }
    AVCaptureDeviceFormat * useFormat = nil;
    CGSize useSize = CGSizeMake(0, 0);
    float minDiff = FLT_MAX;
    for (AVCaptureDeviceFormat * format in self.videoDevice.formats) {
        CMVideoDimensions s = CMVideoFormatDescriptionGetDimensions(format.formatDescription);
        int width = s.width;
        int height = s.height;
        float diff = getRatioError(size.width, width) + getRatioError(size.height, height);
        if (diff < minDiff) {
            minDiff = diff;
            useFormat = format;
            useSize = CGSizeMake(width, height);
        }
    }
    if (useFormat != nil) {
        BOOL success = FALSE;
        if (useSize.width != self.previewSize.width || useSize.height != self.previewSize.height) {
            NSError *error;
            if ([self.videoDevice lockForConfiguration : &error]) {
                self.videoDevice.activeFormat = useFormat;
                success = TRUE;
                [self.videoDevice unlockForConfiguration];
            } else {
                NSLog(@"Fail to lock camera device for configuration : %@",[error description]);
            }
            self.previewSize = useSize;
            easyar_Vec2I *cameraSize = [easyar_Vec2I create:@[@(self.previewSize.width), @(self.previewSize.height)]];
            CameraParams = [easyar_CameraParameters createWithDefaultIntrinsics: cameraSize cameraDeviceType:easyar_CameraDeviceType_Back cameraOrientation:90];
        }
        return success;
    }
    return FALSE;
}

#pragma mark -
#pragma mark Helper Methods
- (AVCaptureDevice*)cameraWithPosition:(AVCaptureDevicePosition)position
{
    NSArray *devices = [AVCaptureDevice devicesWithMediaType : AVMediaTypeVideo];
    for (AVCaptureDevice *device in devices) {
        if ([device position] == position) {
            return device;
        }
    }
    return nil;
}

-(void)addVideoDataOutput
{
    self.captureOutPut = [[AVCaptureVideoDataOutput alloc] init];
    self.captureOutPut.alwaysDiscardsLateVideoFrames = YES;
    
    dispatch_queue_t queue;
    queue = dispatch_queue_create("cn.easyar.engine", DISPATCH_QUEUE_SERIAL);
    [self.captureOutPut setSampleBufferDelegate : self queue : queue];
    
    NSString *key = (NSString *)kCVPixelBufferPixelFormatTypeKey;
    NSNumber *value = [NSNumber numberWithUnsignedInt : kCVPixelFormatType_420YpCbCr8BiPlanarFullRange];
    NSDictionary *settings = @{key:value};
    [self.captureOutPut setVideoSettings : settings];
    
    [self.captureSession addOutput : self.captureOutPut];
}

-(void)captureOutput:(AVCaptureOutput *)captureOutput
didOutputSampleBuffer : (CMSampleBufferRef)sampleBuffer
     fromConnection : (AVCaptureConnection *)connection
{
    (void)captureOutput;
    (void)connection;

    if (CameraParams == nil) { return; }

    CVImageBufferRef imageBuffer = CMSampleBufferGetImageBuffer(sampleBuffer);
    
    OSType bufferPixelFormat = CVPixelBufferGetPixelFormatType(imageBuffer);

    if (bufferPixelFormat != kCVPixelFormatType_420YpCbCr8BiPlanarFullRange)
    {
        return;
    }

    double timestamp = CMTimeGetSeconds(CMSampleBufferGetPresentationTimeStamp(sampleBuffer));

    CVPixelBufferLockBaseAddress(imageBuffer, kCVPixelBufferLock_ReadOnly);
    
    size_t width = CVPixelBufferGetWidth(imageBuffer);
    size_t height = CVPixelBufferGetHeight(imageBuffer);
    
    //https://stackoverflow.com/questions/8838481/kcvpixelformattype-420ypcbcr8biplanarfullrange-frame-to-uiimage-conversion/31553521#31553521
    uint8_t * yBuffer = (uint8_t *)(CVPixelBufferGetBaseAddressOfPlane(imageBuffer, 0));
    size_t yPitch = CVPixelBufferGetBytesPerRowOfPlane(imageBuffer, 0);
    uint8_t * cbCrBuffer = (uint8_t *)(CVPixelBufferGetBaseAddressOfPlane(imageBuffer, 1));
    size_t cbCrPitch = CVPixelBufferGetBytesPerRowOfPlane(imageBuffer, 1);
    
    if (!yBuffer || !cbCrBuffer) {
        CVPixelBufferUnlockBaseAddress(imageBuffer, kCVPixelBufferLock_ReadOnly);
        return;
    }
    
    size_t ySize = width * height;
    size_t cbcrSize = ySize / 2;
    
    size_t bufferSize = width * height * 3 / 2;

    easyar_Buffer *buffer = [easyar_Buffer create: (int)(bufferSize)];

    if (yPitch == width)
    {
        memcpy(buffer.data, yBuffer, ySize);
    }
    else
    {
        uint8_t *src = yBuffer;
        uint8_t *dst = (uint8_t *)(buffer.data);
        for(int i = 0; i < (int)height; i++)
        {
            memcpy(dst, src, width);
            src += yPitch;
            dst += width;
        }
    }
    if (cbCrPitch == width)
    {
        memcpy((uint8_t *)(buffer.data) + ySize, cbCrBuffer, cbcrSize);
    }
    else
    {
        uint8_t *src = cbCrBuffer;
        uint8_t *dst = (uint8_t *)(buffer.data) + ySize;
        for(int i = 0; i < (int)height/2; i++)
        {
            memcpy(dst, src, width);
            src += cbCrPitch;
            dst += width;
        }
    }

    CVPixelBufferUnlockBaseAddress(imageBuffer, kCVPixelBufferLock_ReadOnly);

    easyar_Image *image = [easyar_Image create: buffer format:easyar_PixelFormat_YUV_NV12 width:(int)width height:(int)height];
    
    easyar_InputFrame * frame = [easyar_InputFrame createWithImageAndCameraParametersAndTemporal:image cameraParameters:CameraParams timestamp:timestamp];
    [self.sink handle:frame];
};

-(void)dealloc
{
    if (self.captureOutPut != nil)
        [self.captureSession removeOutput: self.captureOutPut];
    [self.captureSession stopRunning];
}

@end

