#include "SoloGimbal_Parameters.h"
#if HAL_SOLO_GIMBAL_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>

extern const AP_HAL::HAL& hal;

const uint32_t SoloGimbal_Parameters::_retry_period = 3000;
const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5;

SoloGimbal_Parameters::SoloGimbal_Parameters()
{
    reset();
}


void SoloGimbal_Parameters::reset()
{
    memset(_params,0,sizeof(_params));
    _last_request_ms = 0;
    _last_set_ms = 0;
    _flashing_step = GMB_PARAM_NOT_FLASHING;
}

const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param)
{
    switch(param) {
        case GMB_PARAM_GMB_OFF_ACC_X:
            return "GMB_OFF_ACC_X";
        case GMB_PARAM_GMB_OFF_ACC_Y:
            return "GMB_OFF_ACC_Y";
        case GMB_PARAM_GMB_OFF_ACC_Z:
            return "GMB_OFF_ACC_Z";
        case GMB_PARAM_GMB_GN_ACC_X:
            return "GMB_GN_ACC_X";
        case GMB_PARAM_GMB_GN_ACC_Y:
            return "GMB_GN_ACC_Y";
        case GMB_PARAM_GMB_GN_ACC_Z:
            return "GMB_GN_ACC_Z";
        case GMB_PARAM_GMB_OFF_GYRO_X:
            return "GMB_OFF_GYRO_X";
        case GMB_PARAM_GMB_OFF_GYRO_Y:
            return "GMB_OFF_GYRO_Y";
        case GMB_PARAM_GMB_OFF_GYRO_Z:
            return "GMB_OFF_GYRO_Z";
        case GMB_PARAM_GMB_OFF_JNT_X:
            return "GMB_OFF_JNT_X";
        case GMB_PARAM_GMB_OFF_JNT_Y:
            return "GMB_OFF_JNT_Y";
        case GMB_PARAM_GMB_OFF_JNT_Z:
            return "GMB_OFF_JNT_Z";
        case GMB_PARAM_GMB_K_RATE:
            return "GMB_K_RATE";
        case GMB_PARAM_GMB_POS_HOLD:
            return "GMB_POS_HOLD";
        case GMB_PARAM_GMB_MAX_TORQUE:
            return "GMB_MAX_TORQUE";
        case GMB_PARAM_GMB_SND_TORQUE:
            return "GMB_SND_TORQUE";
        case GMB_PARAM_GMB_SYSID:
            return "GMB_SYSID";
        case GMB_PARAM_GMB_FLASH:
            return "GMB_FLASH";
        default:
            return "";
    };
}

void SoloGimbal_Parameters::fetch_params()
{
    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) {
            _params[i].state = GMB_PARAMSTATE_FETCH_AGAIN;
        }
    }
}

bool SoloGimbal_Parameters::initialized()
{
    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) {
            return false;
        }
    }
    return true;
}

bool SoloGimbal_Parameters::received_all()
{
    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) {
            return false;
        }
    }
    return true;
}

void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) {
    if (!_params[param].seen) {
        value = def_val;
    } else {
        value = _params[param].value;
    }
}

void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) {
    if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH &&
       is_equal(_params[param].value,value)) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT ||
       !HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
        return;
    }

    _params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
    _params[param].value = value;

    // make a temporary copy of the ID; mavlink_msg_param_set_send
    // expects an array of the full length
    char tmp_name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN+1] {};
    strncpy(tmp_name, get_param_name(param), MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);

    mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, tmp_name, _params[param].value, MAV_PARAM_TYPE_REAL32);

    _last_set_ms = AP_HAL::millis();
}

void SoloGimbal_Parameters::update()
{
    uint32_t tnow_ms = AP_HAL::millis();

    // retry initial param retrieval
    if(!received_all() && ((tnow_ms - _last_request_ms) > _retry_period) &&
      (HAVE_PAYLOAD_SPACE(_chan, PARAM_REQUEST_LIST))) {
        _last_request_ms = tnow_ms;
        mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
            
        for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
            if (!_params[i].seen) {
                _params[i].fetch_attempts++;
            }
        }
    }

    // retry param_set
    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if (!HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
            break;
        }
        
        if ((_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) && (tnow_ms - _last_set_ms > _retry_period)) {
            mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
            _last_set_ms = AP_HAL::millis();
            
            if (!_params[i].seen) {
                _params[i].fetch_attempts++;
            }
        }
    }

    // check for nonexistent parameters
    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
            _params[i].state = GMB_PARAMSTATE_NONEXISTANT;
            DEV_PRINTF("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
        }
    }

    if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
        bool done = true;
        for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
            if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
                done = false;
                break;
            }
        }

        if (done) {
            _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
            set_param(GMB_PARAM_GMB_FLASH,69.0f);
        }
    }
}

void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t &msg)
{
    mavlink_param_value_t packet;
    mavlink_msg_param_value_decode(&msg, &packet);

#if HAL_LOGGING_ENABLED
    AP_Logger *logger = AP_Logger::get_singleton();
    if (logger != nullptr) {
        logger->Write_Parameter(packet.param_id, packet.param_value);
    }
#endif

    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
        if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
            _params[i].seen = true;
            switch(_params[i].state) {
                case GMB_PARAMSTATE_NONEXISTANT:
                case GMB_PARAMSTATE_NOT_YET_READ:
                case GMB_PARAMSTATE_FETCH_AGAIN:
                    _params[i].value = packet.param_value;
                    _params[i].state = GMB_PARAMSTATE_CONSISTENT;
                    break;
                case GMB_PARAMSTATE_CONSISTENT:
                    _params[i].value = packet.param_value;
                    break;
                case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
                    if (i == GMB_PARAM_GMB_FLASH) {
                        if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && (int)packet.param_value == 1) {
                            _flashing_step = GMB_PARAM_NOT_FLASHING;
                        }
                        _params[i].value = 0;
                        _params[i].state = GMB_PARAMSTATE_CONSISTENT;
                    } else if (is_equal(packet.param_value,_params[i].value)) {
                        _params[i].state = GMB_PARAMSTATE_CONSISTENT;
                    }
                    break;
            }
            break;
        }
    }
}

Vector3f SoloGimbal_Parameters::get_accel_bias()
{
    Vector3f ret;
    get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x);
    get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y);
    get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z);
    return ret;
}
Vector3f SoloGimbal_Parameters::get_accel_gain()
{
    Vector3f ret;
    get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f);
    get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f);
    get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f);
    return ret;
}

void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias)
{
    set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x);
    set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y);
    set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z);
}

void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain)
{
    set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x);
    set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y);
    set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z);
}

Vector3f SoloGimbal_Parameters::get_gyro_bias()
{
    Vector3f ret;
    get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x);
    get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y);
    get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z);
    return ret;
}

void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias)
{
    set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x);
    set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y);
    set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z);
}

Vector3f SoloGimbal_Parameters::get_joint_bias()
{
    Vector3f ret;
    get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x);
    get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y);
    get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z);
    return ret;
}
float SoloGimbal_Parameters::get_K_rate()
{
    float ret;
    get_param(GMB_PARAM_GMB_K_RATE,ret);
    return ret;
}

void SoloGimbal_Parameters::flash()
{
    _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET;
}

bool SoloGimbal_Parameters::flashing()
{
    return _flashing_step != GMB_PARAM_NOT_FLASHING;
}

#endif // HAL_SOLO_GIMBAL_ENABLED