#include <libxmmaque_api.h>
#include <spdlog/spdlog.h>
#include <fmt/format.h>
#include "ptz.h"
#include <mutex>
#include "evutils.hpp"
#include <string.h>

using namespace std;
using namespace evutils;
using namespace jsoncons;

recursive_mutex gMut;
map<string, int> mapDirection = {
    {"left", MOTOR_MOVE_LEFT},
    {"right", MOTOR_MOVE_RIGHT},
    {"up", MOTOR_MOVE_UP},
    {"down", MOTOR_MOVE_DOWN},
    {"upright", MOTOR_MOVE_RIGHT|MOTOR_MOVE_UP},
    {"upleft", MOTOR_MOVE_LEFT|MOTOR_MOVE_UP},
    {"downleft", MOTOR_MOVE_LEFT|MOTOR_MOVE_DOWN},
    {"downright", MOTOR_MOVE_RIGHT|MOTOR_MOVE_DOWN},
    {"rightup", MOTOR_MOVE_RIGHT|MOTOR_MOVE_UP},
    {"leftup", MOTOR_MOVE_LEFT|MOTOR_MOVE_UP},
    {"leftdown", MOTOR_MOVE_LEFT|MOTOR_MOVE_DOWN},
    {"rightdown", MOTOR_MOVE_RIGHT|MOTOR_MOVE_DOWN}
};

void ptz_waitfor_idle()
{
    XM_U32 action,x,y;
    usleep(100*1000);
    int cnt = 5 * 60;  // wait for max 60s
    while (cnt-- > 0) {
        spdlog::info("ptz waiting");
        if (0 == LibXmMaQue_Motor_getPostion(&action,&x,&y)) {
            if (MOTOR_IDLE == action) {
                break;
            }
            else {
                usleep(200*1000);
            }
        }
        else {
            spdlog::error("ptz failed get position when waiting for idle");
            break;
        }
    }

    spdlog::info("ptz end wait");
}

string ptz_service_start()
{
    string rc;
    spdlog::info("start ptz service: calibration/re-locating");
    if (LibXmMaQue_Motor_create() < 0) {
        rc = "ptz failed start service";
        return rc;
    }
    rc = ptz_load();
    if(!rc.empty()) {
        return rc;
    }

    spdlog::info("ptz start service finished.");
    return rc;
}

string ptz_service_stop()
{
    LibXmMaQue_Motor_stop();
    LibXmMaQue_Motor_destroy();
}

string ptz_get_params(ptz_param_position & param)
{
    uint32_t x, y;
    string rc;
    if(LibXmMaQue_Motor_getMaxSteps(&x, &y)< 0) {
        rc = "ptz failed to get max steps";
        return rc;
    }

    uint32_t direction, cx, cy;
    if(LibXmMaQue_Motor_getPostion(&direction, &cx, &cy) < 0) {
        rc = "ptz failed to get current position";
        return rc;
    }
    param.min_x = 0;
    param.max_x = x;
    param.step_x = x/EV_PTZ_X_DEGREE;
    param.current_x_deg = cx / param.step_x;
    param.current_x_steps = cx;
    param.min_y = 0;
    param.max_y = y;
    param.step_y = y/EV_PTZ_Y_DEGREE;
    param.current_y_deg = cy / param.step_y;
    param.current_y_steps = cy;
    spdlog::info("ptz_get_params: max_x: {}, max_y: {}, current_x: {}, current_y: {}, step_x: {}, step_y: {}", param.max_x, param.max_y, param.current_x_steps, param.current_y_steps, param.step_x, param.step_y);

    return rc;
}

static const string extraSysConfigFile = "/etc/evsys.json";

string ptz_save()
{
    ptz_param_position pos;
    string rc;
    rc = ptz_get_params(pos);
    if(!rc.empty()) {
        return rc;
    }

    json js;
    js["ptz"] = json();
    js["ptz"]["x"] = pos.current_x_steps;
    js["ptz"]["y"] = pos.current_y_steps;
    spdlog::info("ptz_save current: {}", js.to_string());
    rc = save_json(js, extraSysConfigFile, true, evutils::make_default_sys_extra_config);
    if(!rc.empty()) {
        spdlog::error(rc);
    }
    spdlog::info("ptz_save current end: {}",  js.to_string());
    return rc;
}

string ptz_load()
{
    string rc;
    json js;
    rc = load_json(js, extraSysConfigFile, evutils::make_default_sys_extra_config);
    if(!rc.empty()) {
        return rc;
    }
    rc = save_json(js, extraSysConfigFile, false, evutils::make_default_sys_extra_config);
    if(!rc.empty()) {
        spdlog::error(rc);
        return rc;
    }
    auto x = js["ptz"]["x"].as<uint32_t>();
    auto y = js["ptz"]["y"].as<uint32_t>();
    rc = ptz_relocate(x, y);
    if(!rc.empty()) {
        spdlog::error(rc);
        return rc;
    }

    spdlog::info("ptz loaded");
    return rc;
}

string ptz_move(string dire, float degree)
{
    string rc;
    {
        lock_guard<recursive_mutex> lk(gMut);

        if(mapDirection.count(dire) == 0) {
            rc = fmt::format("ptz move invalid direction: {}", dire);
            return rc;
        }

        ptz_param_position curr_pos;
        rc = ptz_get_params(curr_pos);
        if(!rc.empty()) {
            return rc;
        }

        if(dire.find("left") != string::npos || dire.find("right") != string::npos) {
            curr_pos.current_x_steps = (degree * curr_pos.step_x);
        }
        else {
            curr_pos.current_x_steps = 0;
        }
        if(dire.find("up")!= string::npos || dire.find("down") != string::npos ) {
            curr_pos.current_y_steps = (degree * curr_pos.step_y);
        }
        else {
            curr_pos.current_y_steps = 0;
        }
        spdlog::info("ptz executing: {}, {}", dire,  degree);

        spdlog::info("motor moving steps: {}, {}", curr_pos.current_x_steps, curr_pos.current_y_steps );

        if(LibXmMaQue_Motor_move(mapDirection[dire], 6, 6, curr_pos.current_x_steps, curr_pos.current_y_steps) < 0) {
            rc = fmt::format("ptz failed to move {} {}: current({},{}), max({},{})", dire, degree, curr_pos.current_x_deg, curr_pos.current_y_deg, 355, 0);
            if(!rc.empty()) {
                return rc;
            }
        }
    }

    ptz_waitfor_idle();
    rc = ptz_save();
    return rc;
}

string ptz_relocate(int x, int y)
{
    lock_guard<recursive_mutex> lk(gMut);
    ptz_param_position param;
    string rc;
    rc = ptz_get_params(param);
    if(!rc.empty()) {
        return rc;
    }
    // reloacate to center
    uint32_t targetx = 0, targety = 0;
    if(x = -1) {
        targetx = param.max_x/2;
    }
    else {
        targetx = x;
    }
    if(y = -1) {
        targety = param.max_y/2;
    }
    else {
        targety = y;
    }

    // move to origin(0,0)
    LibXmMaQue_Motor_setZero();
    spdlog::info("ptz set zero");
    ptz_waitfor_idle();
    spdlog::info("ptz set to zero");

    // move to target (x,y)
    auto i = LibXmMaQue_Motor_move(mapDirection["upright"], 6, 6, targetx, targety);
    if(i< 0) {
        rc = fmt::format("ptz failed move to center: {} {}",  targetx, targety);
        return rc;
    }

    spdlog::info("ptz move to center");
    ptz_waitfor_idle();
    spdlog::info("ptz end center");

    return rc;
}