#include "Camera.h"
#include "Core/Logger.h"
#include "sensorsdk_api.h"
#include <thread>

Camera::Camera() {
    SensorSDK_CameraLens cameraLens = {};
    cameraLens.AFControlModel = 2;

    char boardLine[] = "000";
    char lensType[] = "YT_2812_2MP";
    char autoLensParmPath[] = "/data";
    int status = SensorSdk_Init(7, boardLine, lensType, autoLensParmPath, &cameraLens);
    if (status == 0) {
        LOG(info) << "SensorSdk_Init() successed.";
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    } else {
        LOG(info) << "SensorSdk_Init() failed.";
    }

    SENSOR_SDK_AUTOLENS_PARAM_T parameter = {0};
    parameter.Params.nIrCut = 1;
    status = SensorSdk_AutoLens_SetParam(0, SensorSDK_AutoLens_Type_IRCUT, &parameter);
    if (status == 0) {
        LOG(info) << "SensorSdk_AutoLens_SetParam() successed.";
    } else {
        LOG(info) << "SensorSdk_AutoLens_SetParam() failed.";
    }
    initIrCutGpio();
}

void Camera::initIrCutGpio() {
    constexpr auto initCommand = R"(
        /system/bin/bspmm 0x179f0014 0x1200;
        echo 84 > /sys/class/gpio/export;
        echo out > /sys/class/gpio/gpio84/direction;
        echo 83 > /sys/class/gpio/export;
        echo out > /sys/class/gpio/gpio83/direction;
    )";
    system(initCommand);

    constexpr auto dayCommand = R"(
        echo 0 > /sys/class/gpio/gpio84/value;
        echo 1 > /sys/class/gpio/gpio83/value;
        sleep 1;
        echo 0 > /sys/class/gpio/gpio84/value;
        echo 0 > /sys/class/gpio/gpio83/value;
    )";
    system(dayCommand);
}

bool Camera::zeroCheck() {
    SENSOR_SDK_AUTOLENS_PARAM_T param = {0};
    int status = SensorSdk_AutoLens_SetParam(0, SensorSDK_AutoLens_Type_ZeroCheck, &param);
    LOG(info) << "zero check " << (status == SENSOR_SDK_SUCC ? "success" : "failed");
    return status == SENSOR_SDK_SUCC;
}

bool Camera::focus(Focus type) {
    using namespace std::chrono_literals;
    int focusType = (type == Focus::Far) ? SensorSDK_AutoLens_Type_FocusFar : SensorSDK_AutoLens_Type_FocusNear;

    SENSOR_SDK_AUTOLENS_PARAM_T autolens_param = {0};
    autolens_param.Params.Param_Base.nStop = 0;
    autolens_param.Params.Param_Base.nSpeed = 10;
    int status = SensorSdk_AutoLens_SetParam(0, focusType, &autolens_param);

    std::this_thread::sleep_for(10ms);

    autolens_param.Params.Param_Base.nStop = 1;
    autolens_param.Params.Param_Base.nSpeed = 0;
    status = SensorSdk_AutoLens_SetParam(0, focusType, &autolens_param);
    return status == SENSOR_SDK_SUCC;
}

bool Camera::zoom(Zoom type) {
    using namespace std::chrono_literals;
    int zoomType = (type == Zoom::In) ? SensorSDK_AutoLens_Type_ZoomIn : SensorSDK_AutoLens_Type_ZoomOut;

    SENSOR_SDK_AUTOLENS_PARAM_T autolens_param = {0};
    autolens_param.Params.Param_Base.nStop = 0;
    autolens_param.Params.Param_Base.nSpeed = 10;
    int status = SensorSdk_AutoLens_SetParam(0, zoomType, &autolens_param);

    std::this_thread::sleep_for(10ms);

    autolens_param.Params.Param_Base.nStop = 1;
    autolens_param.Params.Param_Base.nSpeed = 0;
    status = SensorSdk_AutoLens_SetParam(0, zoomType, &autolens_param);

    return status == SENSOR_SDK_SUCC;
}