/*
 * calibration_parser.cpp - parse fisheye calibration file
 *
 *  Copyright (c) 2016-2017 Intel Corporation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Author: Junkai Wu <junkai.wu@intel.com>
 */

#include "calibration_parser.h"
#include "file_handle.h"

namespace XCam {

CalibrationParser::CalibrationParser()
{
}

#define CHECK_NULL(ptr) \
    if(ptr == NULL) { \
        XCAM_LOG_ERROR("Parse file failed"); \
        return XCAM_RETURN_ERROR_FILE; \
    }

XCamReturn
CalibrationParser::parse_intrinsic_param(char *file_body, IntrinsicParameter &intrinsic_param)
{
    char *line_str = NULL;
    char *line_endptr = NULL;
    char *tok_str = NULL;
    char *tok_endptr = NULL;
    static const char *line_tokens = "\r\n";
    static const char *str_tokens = " \t";

    do {
        line_str = strtok_r(file_body, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        intrinsic_param.poly_length = strtol(tok_str, NULL, 10);

        XCAM_FAIL_RETURN (
            ERROR, intrinsic_param.poly_length <= XCAM_INTRINSIC_MAX_POLY_SIZE,
            XCAM_RETURN_ERROR_PARAM,
            "intrinsic poly length:%d is larger than max_size:%d.",
            intrinsic_param.poly_length, XCAM_INTRINSIC_MAX_POLY_SIZE);

        for(uint32_t i = 0; i < intrinsic_param.poly_length; i++) {
            tok_str = strtok_r(NULL, str_tokens, &tok_endptr);
            CHECK_NULL(tok_str);
            intrinsic_param.poly_coeff[i] = (strtof(tok_str, NULL));
        }

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        intrinsic_param.yc = strtof(tok_str, NULL);

        tok_str = strtok_r(NULL, str_tokens, &tok_endptr);
        CHECK_NULL(tok_str);
        intrinsic_param.xc = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        intrinsic_param.c = strtof(tok_str, NULL);

        tok_str = strtok_r(NULL, str_tokens, &tok_endptr);
        CHECK_NULL(tok_str);
        intrinsic_param.d = strtof(tok_str, NULL);

        tok_str = strtok_r(NULL, str_tokens, &tok_endptr);
        CHECK_NULL(tok_str);
        intrinsic_param.e = strtof(tok_str, NULL);
    } while(0);

    return XCAM_RETURN_NO_ERROR;
}

XCamReturn
CalibrationParser::parse_extrinsic_param(char *file_body, ExtrinsicParameter &extrinsic_param)
{
    char *line_str = NULL;
    char *line_endptr = NULL;
    char *tok_str = NULL;
    char *tok_endptr = NULL;
    static const char *line_tokens = "\r\n";
    static const char *str_tokens = " \t";

    do {
        line_str = strtok_r(file_body, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.trans_x = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.trans_y = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.trans_z = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.roll = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.pitch = strtof(tok_str, NULL);

        line_str = strtok_r(NULL, line_tokens, &line_endptr);
        CHECK_NULL(line_str);
        tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        while(tok_str == NULL || tok_str[0] == '#') {
            line_str = strtok_r(NULL, line_tokens, &line_endptr);
            CHECK_NULL(line_str);
            tok_str = strtok_r(line_str, str_tokens, &tok_endptr);
        }
        extrinsic_param.yaw = strtof(tok_str, NULL);
    } while(0);

    return XCAM_RETURN_NO_ERROR;
}

XCamReturn
CalibrationParser::parse_intrinsic_file(const char *file_path, IntrinsicParameter &intrinsic_param)
{
    XCAM_ASSERT (file_path);

    FileHandle file_reader;
    XCamReturn ret = XCAM_RETURN_NO_ERROR;
    std::vector<char> context;
    size_t file_size = 0;

    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.open (file_path, "r")), ret,
        "open intrinsic file(%s) failed.", file_path);
    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.get_file_size (file_size)), ret,
        "read intrinsic file(%s) failed to get file size.", file_path);
    context.resize (file_size + 1);
    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.read_file (&context[0], file_size)), ret,
        "read intrinsic file(%s) failed, file size:%d.", file_path, (int)file_size);
    file_reader.close ();
    context[file_size] = '\0';

    return parse_intrinsic_param (&context[0], intrinsic_param);
}

XCamReturn
CalibrationParser::parse_extrinsic_file(const char *file_path, ExtrinsicParameter &extrinsic_param)
{
    XCAM_ASSERT (file_path);

    FileHandle file_reader;
    XCamReturn ret = XCAM_RETURN_NO_ERROR;
    std::vector<char> context;
    size_t file_size = 0;

    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.open (file_path, "r")), ret,
        "open extrinsic file(%s) failed.", file_path);
    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.get_file_size (file_size)), ret,
        "read extrinsic file(%s) failed to get file size.", file_path);
    context.resize (file_size + 1);
    XCAM_FAIL_RETURN (
        WARNING, xcam_ret_is_ok (ret = file_reader.read_file (&context[0], file_size)), ret,
        "read extrinsic file(%s) failed, file size:%d.", file_path, (int)file_size);
    file_reader.close ();
    context[file_size] = '\0';

    return parse_extrinsic_param (&context[0], extrinsic_param);
}

}