Scheduler::Scheduler(const Scheduler::conf_t config) :
    schedule_strategy_(config.schedule_strategy),
    debug_(config.debug),
    task_count_(0),
    current_schedule_slot_(0)
{
    // allocate memory for tasks
    for(max_task_count_ = config.max_task_count; max_task_count_ > 0; max_task_count_--)
    {
        tasks_ = (Scheduler_task*)malloc(sizeof(Scheduler_task)*max_task_count_);

        if(tasks_ != NULL)
        {
            break;
        }
    }
    if(max_task_count_< config.max_task_count)
    {
        print_util_dbg_print("[Scheduler] constructor: tried to allocate task list for ");
        print_util_dbg_print_num(config.max_task_count,10);
        print_util_dbg_print(" tasks; only space for ");
        print_util_dbg_print_num(max_task_count_,10);
        print_util_dbg_print("\r\n");
    }
}

static mav_result_t state_telemetry_toggle_remote_use(state_t* state, mavlink_command_long_t* packet)
{
	mav_result_t result = MAV_RESULT_UNSUPPORTED;
	
	if ( packet->param1 == 1)
	{
		state->remote_active = 1;
		state->use_mode_from_remote = 1;
		
		print_util_dbg_print("Remote control activated\r\n");
		
		result = MAV_RESULT_ACCEPTED;
	}
	else if (packet->param1 == 0)
	{
		state->remote_active = 0;
		state->use_mode_from_remote = 0;
		
		print_util_dbg_print("Remote control disactivated\r\n");
		
		result = MAV_RESULT_ACCEPTED;
	}
	
	return result;
}

void onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg)
{
	// Onboard parameters storage
	if (msg->param1 == 0)
	{
	 	// read parameters from flash
	 	print_util_dbg_print("Reading from flashc...\r\n");
		if(onboard_parameters_read_parameters_from_flashc(onboard_parameters))
		{
			// TODO: update simulation calibration values
			//simulation_calib_set(&sim_model);
	 	}
	}
	else if (msg->param1 == 1)
	{
	 	// write parameters to flash
	 	//print_util_dbg_print("No Writing to flashc\n");
	 	print_util_dbg_print("Writing to flashc\r\n");
	 	onboard_parameters_write_parameters_to_flashc(onboard_parameters);
	}

	mavlink_message_t ack_msg;
	mavlink_msg_command_ack_pack(	onboard_parameters->mavlink_stream->sysid,
									onboard_parameters->mavlink_stream->compid, 
									&ack_msg,
									MAV_CMD_PREFLIGHT_STORAGE, 
									MAV_RESULT_ACCEPTED	);
	mavlink_stream_send(onboard_parameters->mavlink_stream, &ack_msg);
}

mav_result_t onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg)
{
	mav_result_t result = MAV_RESULT_DENIED;
	
	// Onboard parameters storage
	if (msg->param1 == 0)
	{
	 	// read parameters from flash
	 	print_util_dbg_print("Reading from flashc...\r\n");
		if(onboard_parameters_read_parameters_from_flashc(onboard_parameters))
		{
			result = MAV_RESULT_ACCEPTED;
			// TODO: update simulation calibration values
			//simulation_calib_set(&sim_model);
	 	}
		else
		{
			result = MAV_RESULT_DENIED;
		}
	}
	else if (msg->param1 == 1)
	{
	 	// write parameters to flash
	 	//print_util_dbg_print("No Writing to flashc\n");
	 	print_util_dbg_print("Writing to flashc\r\n");
	 	onboard_parameters_write_parameters_to_flashc(onboard_parameters);
		
		result = MAV_RESULT_ACCEPTED;
	}

	return result;
}

DSTATUS disk_status (BYTE pdrv)
{
	DSTATUS stat = STA_NOINIT;
	int result = RES_ERROR;

	if ((actual_status & STA_NOINIT)||(actual_status & STA_NODISK))
	{
		return STA_NOINIT;
	}

	// Only MMC supported
	if (pdrv!=0)
	{
		return RES_PARERR;
	}
	uint8_t drive_num = MMC;

	switch (drive_num)
	{
		case ATA :
			//result = ATA_disk_status();

			print_util_dbg_print("NO SUPPORTED! ATA status!\r\n");

			// translate the result code here
		
			result = RES_ERROR;
			stat = STA_NODISK;
			break;

		case MMC :
			//result = MMC_disk_status();

			// translate the result code here

			result = sd_spi_status();
		
			if (result)
			{
				stat = 0;
			}
			else
			{
				stat = STA_NODISK;
			}
			break;

		case USB :
			//result = USB_disk_status();

			print_util_dbg_print("NO SUPPORTED! USB status!\r\n");

			// translate the result code here
		
			result = RES_ERROR;
			stat = STA_NODISK;
			break;
	}
	return stat;
}

void onboard_parameters_add_parameter_float(onboard_parameters_t* onboard_parameters, float* val, const char* param_name) 
{
	onboard_parameters_set_t* param_set = onboard_parameters->param_set;
	
	if( val == NULL )
	{
		print_util_dbg_print("[ONBOARD PARAMETER] Error: Null pointer!");
	}
	else
	{
		if( param_set->param_count < param_set->max_param_count )
		{
			onboard_parameters_entry_t* new_param = &param_set->parameters[param_set->param_count];

			new_param->param                     = val;
			strcpy( new_param->param_name, 		param_name );
			new_param->data_type                 = MAV_PARAM_TYPE_REAL32;
			new_param->param_name_length         = strlen(param_name);
			new_param->schedule_for_transmission = true;
			
			param_set->param_count += 1;
		}
		else
		{
			print_util_dbg_print("[ONBOARD PARAMETER] Error: Cannot add more param\r\n");
		}
	}
}

void data_logging_add_parameter_float(data_logging_t* data_logging, float* val, const char* param_name)
{
	data_logging_set_t* data_logging_set = data_logging->data_logging_set;
	
	if( val == NULL )
	{
		print_util_dbg_print("[DATA LOGGING] Error: Null pointer!");
	}
	else
	{
		if( data_logging_set->data_logging_count < data_logging_set->max_data_logging_count )
		{
			data_logging_entry_t* new_param = &data_logging_set->data_log[data_logging_set->data_logging_count];

			new_param->param					 = (double*) val;
			strcpy( new_param->param_name, 		 param_name );
			new_param->data_type                 = MAV_PARAM_TYPE_REAL32;
			
			data_logging_set->data_logging_count += 1;
		}
		else
		{
			print_util_dbg_print("[DATA LOGGING] Error: Cannot add more logging param.\r\n");
		}
	}
}

void scheduler_init(scheduler_t* scheduler, const scheduler_conf_t* config, const mavlink_stream_t* mavlink_stream) 
{
	// Init dependency
	scheduler->mavlink_stream = mavlink_stream;

	// Init schedule strategy
	scheduler->schedule_strategy = config->schedule_strategy;

	// Init debug mode
	scheduler->debug = config->debug;

	// Allocate memory for the task set
	scheduler->task_set = malloc( sizeof(task_set_t) + sizeof(task_entry_t[config->max_task_count]) );
	if ( scheduler->task_set != NULL ) 
	{
		scheduler->task_set->max_task_count = config->max_task_count;
	}
	else
	{
		print_util_dbg_print("[SCHEDULER] ERROR ! Bad memory allocation\r\n");
		scheduler->task_set->max_task_count = 0;		
	}

	scheduler->task_set->task_count = 0;
	scheduler->task_set->current_schedule_slot = 0;
	
	print_util_dbg_print("[SCHEDULER] Init\r\n");
}

static mode_flag_armed_t get_armed_flag(remote_t* remote)
{
    const remote_mode_t* remote_mode = &remote->mode;
    mode_flag_armed_t armed = remote_mode->current_desired_mode.ARMED;

    // Get armed flag
    if( remote_get_throttle(remote) < -0.95f &&
            remote_get_yaw(remote) > 0.9f &&
            remote_get_pitch(remote) > 0.9f &&
            remote_get_roll(remote) > 0.9f )
    {
        // Both sticks in bottom right corners => arm
        print_util_dbg_print("Arming!\r\n");
        armed = ARMED_ON;
    }
    else if ( remote_get_throttle(remote) < -0.95f &&
              remote_get_yaw(remote) < -0.9f &&
              remote_get_pitch(remote) > 0.9f &&
              remote_get_roll(remote) < -0.9f )
    {
        // Both sticks in bottom left corners => disarm
        print_util_dbg_print("Disarming!\r\n");
        armed = ARMED_OFF;
    }
    else
    {
        // Keep current flag
    }

    return armed;
}

DSTATUS disk_initialize (BYTE pdrv)
{
	DSTATUS stat = STA_NOINIT;
	int result;

	actual_status = STA_NOINIT;

	uint8_t drive_num;
	if (pdrv==0)
	{
		drive_num = MMC;
	}
	else
	{
		return STA_NOINIT;
	}



	switch (drive_num)
	{
		case ATA :
			//result = ATA_disk_initialize();

			print_util_dbg_print("NO SUPPORTED! ATA init!\r\n");

			// translate the result code here
			result = RES_ERROR;
			stat = STA_NODISK;
			break;

		case MMC :
			//result = MMC_disk_initialize();
		
			result = sd_spi_init();
		
			if (result)
			{
				stat = 0;
				actual_status = 0;
			}
			else
			{
				stat = STA_NOINIT;
			}
			break;

		case USB :
			//result = USB_disk_initialize();

			print_util_dbg_print("NO SUPPORTED! USB init!\r\n");
		
			// translate the result code here
			result = RES_ERROR;
			stat = STA_NODISK;
			break;
	}
	return stat;
}

void neighbors_selection_extrapolate_or_delete_position(neighbors_t *neighbors)
{
	int32_t i, ind, ind_sup;
	uint32_t delta_t;
	
	uint32_t actual_time = time_keeper_get_millis();
	
	for (ind = 0; ind < neighbors->number_of_neighbors; ind++)
	{
		delta_t = actual_time- neighbors->neighbors_list[ind].time_msg_received;

		if (delta_t >= NEIGHBOR_TIMEOUT_LIMIT_MS)
		{
			print_util_dbg_print("Suppressing neighbor number ");
			print_util_dbg_print_num(ind,10);
			print_util_dbg_print("\r\n");
			
			// suppressing element ind
			for (ind_sup = ind; ind_sup < (neighbors->number_of_neighbors - 1); ind_sup++)
			{
				neighbors->neighbors_list[ind_sup] = neighbors->neighbors_list[ind_sup + 1];
			}
			(neighbors->number_of_neighbors)--;
			
		}
		else if (delta_t > ORCA_TIME_STEP_MILLIS)
		{
			// extrapolating the last known position assuming a constant velocity
			
			for(i = 0; i < 3; i++)
			{
				neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i] + neighbors->neighbors_list[ind].velocity[i] *((float)delta_t/1000);
			}
			//print_util_dbg_print("Extrapolated position (x100):");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10);
			//print_util_dbg_print(", ");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10);
			//print_util_dbg_print(", ");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10);
			//print_util_dbg_print(")");
		}
		else
		{
			// taking the latest known position
			for (i = 0; i < 3; i++)
			{
				neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i];
			}
			//print_util_dbg_print("Last known position (x100):");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10);
			//print_util_dbg_print(", ");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10);
			//print_util_dbg_print(", ");
			//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10);
			//print_util_dbg_print(")");
		}
	}
}

bool scheduler_add_task(scheduler_t* scheduler, uint32_t repeat_period, task_run_mode_t run_mode, task_timing_mode_t timing_mode, task_priority_t priority, task_function_t call_function, task_argument_t function_argument, uint32_t task_id) 
{
	bool task_successfully_added = false;
	task_set_t* ts = scheduler->task_set;

	// Check if the scheduler is not full
	if ( ts->task_count < ts->max_task_count ) 
	{
		// Check if there is already a task with this ID
		bool id_is_unique = true;
		for (uint32_t i = 0; i < ts->task_count; ++i)
		{
			if ( ts->tasks[i].task_id == task_id )
			{
				id_is_unique = false;
				break;
			}
		}

		// Add new task
		if ( id_is_unique == true )
		{
			task_entry_t* new_task = &ts->tasks[ts->task_count];

			new_task->call_function     = call_function;
			new_task->function_argument = function_argument;
			new_task->task_id           = task_id;		
			new_task->run_mode          = run_mode;
			new_task->timing_mode       = timing_mode;	
			new_task->priority          = priority;	
			new_task->repeat_period     = repeat_period;
			new_task->next_run          = time_keeper_get_micros();
			new_task->execution_time    = 0;
			new_task->delay_max         = 0;
			new_task->delay_avg         = 0;
			new_task->delay_var_squared = 0;

			ts->task_count += 1;

			task_successfully_added = true;
		}
		else
		{
			print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n");
			task_successfully_added = false;
		}
	}
	else
	{
		print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n");
		task_successfully_added = false;
	}

	return task_successfully_added;
}

void print_util_dbg_log_value(const char* msg, int32_t value, char base) 
{
	print_util_dbg_print(msg);
	
	if (base>1) 
	{
		print_util_dbg_print_num(value, base);
	}
	
	print_util_dbg_print("\n");
}

void print_util_dbg_init_msg(const char* module_name, bool init_success)
{
    print_util_dbg_print(module_name);
    if (init_success == true)
    {
        print_util_dbg_print(" ok\r\n");
    }
    else
    {
        print_util_dbg_print(" INIT ERROR\r\n");
    }
}

static mav_result_t position_estimation_set_new_home_position(position_estimation_t *pos_est, mavlink_command_long_t* packet)
{
	mav_result_t result;
	
	if(pos_est->state->mav_mode.ARMED == ARMED_OFF)
	{
		if (packet->param1 == 1)
		{
			// Set new home position to actual position
			print_util_dbg_print("Set new home location to actual position.\r\n");
			pos_est->local_position.origin = coord_conventions_local_to_global_position(pos_est->local_position);

			print_util_dbg_print("New Home location: (");
			print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
			print_util_dbg_print(", ");
			print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
			print_util_dbg_print(", ");
			print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
			print_util_dbg_print(")\r\n");
		}
		else
		{
			// Set new home position from msg
			print_util_dbg_print("[POSITION ESTIMATION] Set new home location. \r\n");

			pos_est->local_position.origin.latitude = packet->param5;
			pos_est->local_position.origin.longitude = packet->param6;
			pos_est->local_position.origin.altitude = packet->param7;

			print_util_dbg_print("New Home location: (");
			print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
			print_util_dbg_print(", ");
			print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
			print_util_dbg_print(", ");
			print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
			print_util_dbg_print(")\r\n");
		}

		pos_est->fence_set = false;
		position_estimation_set_new_fence_origin(pos_est);

		*pos_est->nav_plan_active = false;
	
		result = MAV_RESULT_ACCEPTED;
	}
	else
	{
		result = MAV_RESULT_TEMPORARILY_REJECTED;
	}
	
	return result;
}

void mavlink_message_handler_msg_default_dbg(mavlink_message_t* msg)
{
	if ((msg->sysid == MAVLINK_BASE_STATION_ID)&&(msg->msgid != MAVLINK_MSG_ID_MANUAL_CONTROL))
	{
		print_util_dbg_print("Received message with ID");
		print_util_dbg_print_num(msg->msgid, 10);
		print_util_dbg_print(" from system");
		print_util_dbg_print_num(msg->sysid, 10);
		print_util_dbg_print(" from component");
		print_util_dbg_print_num(msg->compid,10);
		print_util_dbg_print("\r\n");
	}
}

bool Scheduler::add_task(uint32_t repeat_period,
              Scheduler_task::task_function_t call_function,
              Scheduler_task::task_argument_t function_argument,
              Scheduler_task::priority_t priority,
              Scheduler_task::timing_mode_t timing_mode,
              Scheduler_task::run_mode_t run_mode,
              int32_t task_id)
{
    bool task_successfully_added = false;

    // Check if the scheduler is not full
    if (task_count_ < max_task_count_)
    {
        if (task_id == -1)
        {
           task_id = task_count_;
        }

        // Check if there is already a task with this ID
        bool id_is_unique = true;
        for (uint32_t i = 0; i < task_count_; ++i)
        {
            if (tasks_[i].get_id() == task_id)
            {
                id_is_unique = false;
                break;
            }
        }

        // Add new task
        if (id_is_unique == true)
        {
            tasks_[task_count_++] = Scheduler_task(repeat_period, run_mode, timing_mode, priority, call_function, function_argument, task_id);
            task_successfully_added = true;
        }
        else
        {
            print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n");
            task_successfully_added = false;
        }
    }
    else
    {
        print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n");
        task_successfully_added = false;
    }

    return task_successfully_added;
}

void state_telemetry_set_mav_mode(state_t* state, uint32_t sysid, mavlink_message_t* msg)
{
	mavlink_set_mode_t packet;
	
	mavlink_msg_set_mode_decode(msg,&packet);
	
	// Check if this message is for this system and subsystem
	// No component ID in mavlink_set_mode_t so no control
	if ((uint8_t)packet.target_system == (uint8_t)sysid)
	{
		print_util_dbg_print("base_mode:");
		print_util_dbg_print_num(packet.base_mode,10);
		print_util_dbg_print(", custom mode:");
		print_util_dbg_print_num(packet.custom_mode,10);
		print_util_dbg_print("\r\n");

		mav_mode_t new_mode;
		new_mode.byte = packet.base_mode;
		
		if (new_mode.ARMED == ARMED_ON)
		{
			if (state->mav_mode.ARMED == ARMED_OFF)
			{
				state_switch_to_active_mode(state, &state->mav_state);
			}
		}
		else
		{
			state->mav_state = MAV_STATE_STANDBY;
		}
		
		state->mav_mode.ARMED = new_mode.ARMED;
		state->mav_mode.MANUAL = new_mode.MANUAL;
		//state->mav_mode.HIL = new_mode.HIL;
		state->mav_mode.STABILISE = new_mode.STABILISE;
		state->mav_mode.GUIDED = new_mode.GUIDED;
		state->mav_mode.AUTO = new_mode.AUTO;
		state->mav_mode.TEST = new_mode.TEST;
		state->mav_mode.CUSTOM = new_mode.CUSTOM;
		
		//state->mav_mode_custom = packet.custom_mode;
		
		print_util_dbg_print("New mav mode:");
		print_util_dbg_print_num(state->mav_mode.byte,10);
		print_util_dbg_print("\r");
		
	}
}

void Data_logging::add_header_name(void)
{
    bool init = true;

    uint16_t i;

    init &= console_.write("time");
    put_r_or_n(0);

    for (i = 0; i < data_logging_count_; i++)
    {
        data_logging_entry_t* param = &data_log_[i];

        init &= console_.write(reinterpret_cast<uint8_t*>(param->param_name), strlen(param->param_name));

        if (!init)
        {
            break;
            if (debug_)
            {
                print_util_dbg_print("Error appending header!\r\n");
            }
        }
        else
        {
            put_r_or_n(i);
        }
    }
    file_init_ = init;
}

bool stabilisation_copter_init(stabilisation_copter_t* stabilisation_copter, stabilisation_copter_conf_t* stabiliser_conf, control_command_t* controls, const imu_t* imu, const ahrs_t* ahrs, const position_estimation_t* pos_est,servos_t* servos)
{
	bool init_success = true;
	
	//init dependencies
	stabilisation_copter->stabiliser_stack = stabiliser_conf->stabiliser_stack;
	stabilisation_copter->motor_layout = stabiliser_conf->motor_layout;
	stabilisation_copter->controls = controls;
	stabilisation_copter->imu = imu;
	stabilisation_copter->ahrs = ahrs;
	stabilisation_copter->pos_est = pos_est;
	stabilisation_copter->servos = servos;
	
	//init controller
	controls->control_mode = ATTITUDE_COMMAND_MODE;
	controls->yaw_mode = YAW_RELATIVE;
	
	controls->rpy[ROLL] = 0.0f;
	controls->rpy[PITCH] = 0.0f;
	controls->rpy[YAW] = 0.0f;
	controls->tvel[X] = 0.0f;
	controls->tvel[Y] = 0.0f;
	controls->tvel[Z] = 0.0f;
	controls->theading = 0.0f;
	controls->thrust = -1.0f;
	
	stabilisation_copter->thrust_hover_point = stabiliser_conf->thrust_hover_point;

	print_util_dbg_print("[STABILISATION COPTER] initalised.\r\n");
	
	return init_success;
}