#include "fts_control.hpp"
#include "px4_platform_common/board_common.h"



FTSController::FTSController() :
	ModuleParams(nullptr),
	ScheduledWorkItem("fts_controller", px4::wq_configurations::lp_default),
	_cycle_perf(perf_alloc(PC_ELAPSED, "fts_controller"": cycle")),
	_interval_perf(perf_alloc(PC_INTERVAL, "fts_controller"": interval"))
{
}

FTSController::~FTSController()
{
	ScheduleClear();
	perf_free(_cycle_perf);
	perf_free(_interval_perf);
}

bool
FTSController::init()
{
	// Update parameters
	if (!update_param()) {
		PX4_WARN("fts_control failed to get necessary parameters.");
		return false;
	}
    uint8_t guid[PX4_GUID_BYTE_LENGTH];
    board_get_px4_guid(guid);
    fts_id.uidBytes[0]=guid[14];
    fts_id.uidBytes[1]=guid[15];
    fts_id.uidBytes[2]=guid[16];
    fts_id.uidBytes[3]=guid[17];

    get_instance()->reset();

	// Task run frequency
	ScheduleOnInterval(500_ms);
	return true;
}

void
FTSController::Run() {
    if (should_exit()) {
        ScheduleClear();
        exit_and_cleanup();
        return;
    }

    perf_begin(_cycle_perf);
    perf_count(_interval_perf);

    // Update AUTO-FTS status
    //TODO manage comm timeout
    if (_auto_fts_message_sub.updated()) {
        _auto_fts_message_sub.copy(&_auto_fts_message);
        _auto_fts_timestamp_last = _auto_fts_message.timestamp;
        //ARMING CONDITIONS ARE MET
        switch (_auto_fts_state){
            case AUTO_FTS_STATE_ARMED:{
                //AFTS IS CURRENTLY ARMED
                //TODO ADD GEOFENCE LOADED CHECK ETC
                //if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
                if (_auto_fts_message.gps_lock != 0) {
                    // FIRING CONTITIONS ARE MET
                    if (_auto_fts_message.geofence_violated != 0) {
                        //GEOFENCE VIOLATED, FIRE
                        _auto_fts_state = AUTO_FTS_STATE_FIRE;
                    } else {
                        //GEOFENCE NOT VIOLATED, REMAIN ARMED AND READY
                    }
                } else {
                    //FIRING CONDITIONS ARE LOST, DISARM AUTO-FTS
                    _auto_fts_state = AUTO_FTS_STATE_DISARMED;
                }
            }break;

            case AUTO_FTS_STATE_DISARMED:
            {
                //AFTS IS NOT CURRENTLY ARMED
                //TODO ADD GEOFENCE LOADED CHECK ETC
                //if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
                if (_auto_fts_message.gps_lock != 0) {
                    if (_auto_fts_message.geofence_violated != 0) {
                        // FAILSAFE: CANNOT ARM AUTO-FTS WHEN GEOFENCE IS ALREADY VIOLATED
                        _auto_fts_state = AUTO_FTS_STATE_ERROR;
                    } else {
                        // ARM AUTO-FTS
                        _auto_fts_state = AUTO_FTS_STATE_ARMED;
                    }
                }else {
                    // REMAIN DISARMED
                }
            }break;

            case AUTO_FTS_STATE_FIRE:
            case AUTO_FTS_STATE_ERROR:
            default: {
                //DO NOTHING
            }break;
        }
    } else{
        if (hrt_elapsed_time(&_auto_fts_timestamp_last) > AUTO_FTS_COMM_TIMEOUT){
            _auto_fts_state = AUTO_FTS_STATE_DISARMED;
        }
    }


    // Manage communication with GROUND-FTS module and update GROUND-FTS status
    if (_ground_fts_message_sub.updated()) {
        _ground_fts_message_sub.copy(&_ground_fts_message);

        if (_ground_fts_message.message == PING) {
            //PING COMMAND
            if (_ground_fts_message.fts_uid == fts_id.uid) {
                //PING RECEIVED, GROUND-FTS IS ARMED
                _ground_fts_state = GROUND_FTS_STATE_ARMED;

                _ground_fts_response.message = PONG;
                _ground_fts_response.fts_uid = fts_id.uid;
                _ground_fts_response_pub.publish(_ground_fts_response);

                _ground_fts_timestamp_last = _ground_fts_message.timestamp;
            }
        } else if (_ground_fts_message.message == PAIR) {
            //PAIR COMMAND
            _ground_fts_response.message = PAIR;
            _ground_fts_response.fts_uid = fts_id.uid;
            _ground_fts_response_pub.publish(_ground_fts_response);
            int32_t param_val = 0;
            if (param_get(param_find("GFTS_PAIRED"), &param_val)==0){
                param_val = 1;
                param_set(param_find("GFTS_PAIRED"), &param_val);
            }
        }

    } else {
        if (hrt_elapsed_time(&_ground_fts_timestamp_last) > GROUND_FTS_COMM_TIMEOUT){
            _ground_fts_state = GROUND_FTS_STATE_DISARMED;
        }
    }

    if (_emergency_command_sub.updated()) {

        _emergency_command_sub.copy(&_emergency_command);

        if (_emergency_command.param == fts_id.uid) {
            if (_emergency_command.emergency_command == ACTIVATE_FTS) {
                //TERMINATE COMMAND RECEIVED

                if(_ground_fts_state == GROUND_FTS_STATE_ARMED) {
                    _ground_fts_state = GROUND_FTS_STATE_FIRE;
                }else{
                    // FAILSAFE: CANNOT FIRE GROUND-FTS IF CURRENTLY DISARMED
                    _ground_fts_state = GROUND_FTS_STATE_ERROR;
                }

            }
        }

    }

    switch (_ground_fts_state) {
        case GROUND_FTS_STATE_DISARMED:{

        }break;

        case GROUND_FTS_STATE_ARMED:{

        }break;

        case GROUND_FTS_STATE_FIRE:
        case GROUND_FTS_STATE_ERROR:
        default:
        {
            //DO NOTHING
        }break;
    }

    switch (_main_fts_state) {
        case MAIN_FTS_STATE_DISARMED:{

            if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
                _main_fts_state = MAIN_FTS_STATE_ERROR;
            }

            else if (_ground_fts_state == GROUND_FTS_STATE_ARMED || _auto_fts_state ==AUTO_FTS_STATE_ARMED){
                get_instance()->arm();
            }

        }break;
        case MAIN_FTS_STATE_ARMED:{
            if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
                get_instance()->fire();
            } else if (_ground_fts_state == GROUND_FTS_STATE_DISARMED && _auto_fts_state ==AUTO_FTS_STATE_DISARMED){
                get_instance()->disarm();
            }
        }break;
        case MAIN_FTS_STATE_FIRE:
        case MAIN_FTS_STATE_ERROR:
        default:
        {
            //DO NOTHING
        }break;
    }


    // Publish new status
    update_topics();

    perf_end(_cycle_perf);

}

void
FTSController::update_topics()
{
    // Publish FTS controller state
    _main_fts_status_msg.timestamp = hrt_absolute_time();
    _main_fts_status_msg.main_fts_state = _main_fts_state;
    _main_fts_status_msg.ground_fts_state = _ground_fts_state;
    _main_fts_status_msg.auto_fts_state = _auto_fts_state;
    _main_fts_status_pub.publish(_main_fts_status_msg);
}

bool FTSController::update_param()
{
    param_get(param_find("GFTS_PAIRED"), &_ground_fts_paired);
    return 1;
}

int
FTSController::task_spawn(int argc, char *argv[])
{
	FTSController *instance = new FTSController();

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

uint8_t
FTSController::fire()
{
    if (_main_fts_state == MAIN_FTS_STATE_ARMED) {
        fts_drv.fts_on();
        _main_fts_state = MAIN_FTS_STATE_FIRE;
        return 0;
    }else{
        return 1;
    }
}

uint8_t
FTSController::reset()
{
    _main_fts_state = MAIN_FTS_STATE_DISARMED;
    _ground_fts_state = GROUND_FTS_STATE_DISARMED;
    _auto_fts_state = AUTO_FTS_STATE_DISARMED;
    fts_drv.fts_off();
    fts_drv.fts_disarm();
    return 0;
}

uint8_t
FTSController::arm()
{
    fts_drv.fts_arm();
    _main_fts_state = MAIN_FTS_STATE_ARMED;
    return 0;
}

uint8_t
FTSController::disarm()
{
    fts_drv.fts_disarm();
    _main_fts_state = MAIN_FTS_STATE_DISARMED;
    return 0;
}

int
FTSController::custom_command(int argc, char *argv[])
{
    if (!strcmp(argv[0], "fire")) {
        get_instance()->fire();
        return 0;
    }

    if (!strcmp(argv[0], "reset")) {
        get_instance()->reset();
        return 0;
    }

    if (!strcmp(argv[0], "arm")) {
        get_instance()->arm();
        return 0;
    }

    if (!strcmp(argv[0], "disarm")) {
        get_instance()->disarm();
        return 0;
    }

    return print_usage("unknown command");
}

int
FTSController::print_status()
{
	perf_print_counter(_cycle_perf);
	perf_print_counter(_interval_perf);

    PX4_INFO("Main FTS module UID:%li", fts_id.uid);
	PX4_INFO("Main FTS state:%s", mfts_status_str(_main_fts_state));
    PX4_INFO("Ground module state:%s", gfts_status_str(_ground_fts_state));
    PX4_INFO("Auto module state:%s", afts_status_str(_auto_fts_state));
	return 0;
}

int
FTSController::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("fts_controller", "system");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_COMMAND("status");
    PRINT_MODULE_USAGE_COMMAND("arm");
    PRINT_MODULE_USAGE_COMMAND("disarm");
    PRINT_MODULE_USAGE_COMMAND("fire");
    PRINT_MODULE_USAGE_COMMAND("reset");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

extern "C" __EXPORT int fts_controller_main(int argc, char *argv[])
{
	return FTSController::main(argc, argv);
}