// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//	************************************************************************************
//	This function gets critical data from EEPROM to get us underway if restarting in air
//	************************************************************************************

// Macro functions
// ---------------
void read_EEPROM_startup(void)
{
	read_EEPROM_PID();
	read_EEPROM_frame();
	read_EEPROM_configs();
	read_EEPROM_flight_modes();
	read_EEPROM_waypoint_info();
	imu.load_gyro_eeprom();
	imu.load_accel_eeprom();
	read_EEPROM_alt_RTL();

	// magnatometer
	read_EEPROM_mag();
	read_EEPROM_mag_declination();
	read_EEPROM_mag_offset();
}

void read_EEPROM_airstart_critical(void)
{
	int16_t temp = 0;
	imu.load_gyro_eeprom();
	imu.load_accel_eeprom();
	
	// Read the home location
	//-----------------------	
	home = get_wp_with_index(0);
		
	// Read pressure sensor values
	//----------------------------
	read_EEPROM_pressure();
}

void save_EEPROM_groundstart(void)
{
	rc_1.save_trim();
	rc_2.save_trim();
	rc_3.save_trim();
	rc_4.save_trim();
	rc_5.save_trim();
	rc_6.save_trim();
	rc_7.save_trim();
	rc_8.save_trim();
	
	// pressure sensor data saved by init_home
}

/********************************************************************************/

long read_alt_to_hold()
{	
	read_EEPROM_alt_RTL();
	if(alt_to_hold == -1)
		return current_loc.alt;
	else
		return alt_to_hold + home.alt;
}

/********************************************************************************/

void save_EEPROM_alt_RTL(void)
{
	eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
}

void read_EEPROM_alt_RTL(void)
{
	alt_to_hold = eeprom_read_dword((const uint32_t *)	EE_ALT_HOLD_HOME);
}

/********************************************************************************/

void read_EEPROM_waypoint_info(void)
{
	wp_total 		= eeprom_read_byte((uint8_t *) EE_WP_TOTAL);				
	wp_radius 		= eeprom_read_byte((uint8_t *) EE_WP_RADIUS);
	loiter_radius 	= eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);
}

void save_EEPROM_waypoint_info(void)
{
	eeprom_write_byte((uint8_t *)	EE_WP_TOTAL, 		wp_total);
	eeprom_write_byte((uint8_t *)	EE_WP_RADIUS, 		wp_radius);
	eeprom_write_byte((uint8_t *)	EE_LOITER_RADIUS, 	loiter_radius);
}

/********************************************************************************/

void read_EEPROM_nav(void)
{
	// for nav estimation
	distance_gain 		= read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
	altitude_gain 		= read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);

	// stored as degree * 100
	x_track_gain 		= read_EE_compressed_float(EE_XTRACK_GAIN, 4);
	x_track_angle 		= eeprom_read_word((uint16_t *)	EE_XTRACK_ANGLE) * 100;
	pitch_max 			= eeprom_read_word((uint16_t *) EE_PITCH_MAX);	// stored as degress * 100
}

void save_EEPROM_nav(void)
{
	// for nav estimation
	write_EE_compressed_float(altitude_gain, 	EE_ALTITUDE_GAIN, 	4);
	write_EE_compressed_float(distance_gain, 	EE_DISTANCE_GAIN, 	4);
	write_EE_compressed_float(x_track_gain, 	EE_XTRACK_GAIN, 	4);
	
	// stored as degrees
 	eeprom_write_word((uint16_t *)	EE_XTRACK_ANGLE, x_track_angle / 100);

	// stored as degrees
	eeprom_write_word((uint16_t *)	EE_PITCH_MAX, pitch_max);
}

/********************************************************************************/

void read_EEPROM_PID(void)
{
	pid_acro_rate_roll.load_gains();
	pid_acro_rate_pitch.load_gains();
	pid_acro_rate_yaw.load_gains();
	
	pid_stabilize_roll.load_gains();
	pid_stabilize_pitch.load_gains();
	pid_yaw.load_gains();
	
	pid_nav_lon.load_gains();
	pid_nav_lat.load_gains();
	pid_baro_throttle.load_gains();
	pid_sonar_throttle.load_gains();
	
	// roll pitch
	stabilize_dampener 			= read_EE_compressed_float(EE_STAB_DAMPENER, 4);

	// yaw
	hold_yaw_dampener			= read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
 	init_pids();
}

void save_EEPROM_PID(void)
{
	pid_acro_rate_roll.save_gains(); 
	pid_acro_rate_pitch.save_gains();
	pid_acro_rate_yaw.save_gains();
	
	pid_stabilize_roll.save_gains();
	pid_stabilize_pitch.save_gains();
	pid_yaw.save_gains();
	
	pid_nav_lon.save_gains();
	pid_nav_lat.save_gains();
	pid_baro_throttle.save_gains();
	pid_sonar_throttle.save_gains();

	// roll pitch
	write_EE_compressed_float(stabilize_dampener,	EE_STAB_DAMPENER, 4);

	// yaw
	write_EE_compressed_float(hold_yaw_dampener, 	EE_HOLD_YAW_DAMPENER, 4);
}

/********************************************************************************/

void save_EEPROM_frame(void)
{
	eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
}

void read_EEPROM_frame(void)
{
	frame_type 	= eeprom_read_byte((uint8_t *)	EE_FRAME);
}

/********************************************************************************/

void save_EEPROM_throttle_cruise(void)
{
	eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, throttle_cruise);
}

void read_EEPROM_throttle_cruise(void)
{
	throttle_cruise 	= eeprom_read_word((uint16_t *)	EE_THROTTLE_CRUISE);
}

/********************************************************************************/

void save_EEPROM_mag_declination(void)
{
	write_EE_compressed_float(mag_declination, 	EE_MAG_DECLINATION, 1);
}

void read_EEPROM_mag_declination(void)
{
	mag_declination 	= read_EE_compressed_float(EE_MAG_DECLINATION, 1);
}

/********************************************************************************/

void save_EEPROM_current_sensor(void)
{
	eeprom_write_byte((uint8_t *)	EE_CURRENT_SENSOR, 	current_sensor);
}

void read_EEPROM_current_sensor(void)
{
	current_sensor	= eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
}

/********************************************************************************/

void save_EEPROM_mag_offset(void)
{
	write_EE_compressed_float(mag_offset_x, 	EE_MAG_X, 2);
	write_EE_compressed_float(mag_offset_y, 	EE_MAG_Y, 2);
	write_EE_compressed_float(mag_offset_z, 	EE_MAG_Z, 2);
}

void read_EEPROM_mag_offset(void)
{
	mag_offset_x 	= read_EE_compressed_float(EE_MAG_X, 2);
	mag_offset_y 	= read_EE_compressed_float(EE_MAG_Y, 2);
	mag_offset_z 	= read_EE_compressed_float(EE_MAG_Z, 2);
}

/********************************************************************************/

void read_EEPROM_mag(void)
{
	compass_enabled	= eeprom_read_byte((uint8_t *) EE_COMPASS);
}

void save_EEPROM_mag(void)
{
	eeprom_write_byte((uint8_t *)	EE_COMPASS, 	compass_enabled);
}

/********************************************************************************/

void save_command_index(void)
{
	eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index);
}

void read_command_index(void)
{
	wp_index = command_must_index 	= eeprom_read_byte((uint8_t *) EE_WP_INDEX);
}

/********************************************************************************/

void save_EEPROM_pressure(void)
{
	eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, 	abs_pressure_ground);
	eeprom_write_word((uint16_t *)EE_GND_TEMP, 			ground_temperature);
}

void read_EEPROM_pressure(void)
{
	abs_pressure_ground = eeprom_read_dword((uint32_t *) 	EE_ABS_PRESS_GND);
	// Better than zero for an air start value
	abs_pressure	 	= abs_pressure_ground;
	ground_temperature 	= eeprom_read_word((uint16_t *) 	EE_GND_TEMP);
}

/********************************************************************************/

void read_EEPROM_radio(void)
{
	rc_1.load_eeprom();
	rc_2.load_eeprom();
	rc_3.load_eeprom();
	rc_4.load_eeprom();
	rc_5.load_eeprom();
	rc_6.load_eeprom();
	rc_7.load_eeprom();
	rc_8.load_eeprom();
}

void save_EEPROM_radio(void)
{	
	rc_1.save_eeprom();
	rc_2.save_eeprom();
	rc_3.save_eeprom();
	rc_4.save_eeprom();
	rc_5.save_eeprom();
	rc_6.save_eeprom();
	rc_7.save_eeprom();
	rc_8.save_eeprom();
}

/********************************************************************************/
// configs are the basics
void read_EEPROM_configs(void)
{
	throttle_min 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MIN);
	throttle_max 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MAX);
	read_EEPROM_throttle_cruise();
	throttle_failsafe_enabled 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE);
	throttle_failsafe_action 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE_ACTION);
	throttle_failsafe_value 	= eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE);
	log_bitmask 				= eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
}

void save_EEPROM_configs(void)
{
	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MIN, 			throttle_min);
	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MAX, 			throttle_max);
	save_EEPROM_throttle_cruise();
	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE,		throttle_failsafe_enabled);
	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action);
	eeprom_write_word((uint16_t *) 	EE_THROTTLE_FS_VALUE,		throttle_failsafe_value);
	eeprom_write_word((uint16_t *) 	EE_LOG_BITMASK,				log_bitmask);
}

/********************************************************************************/

void read_EEPROM_flight_modes(void)
{
	// Read Radio min/max settings
	eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes));
}

void save_EEPROM_flight_modes(void)
{
	// Save Radio min/max settings
 	eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes));
}

/********************************************************************************/

float
read_EE_float(int address)
{
	union {
		byte bytes[4];
		float value;
	} _floatOut;
	
	for (int i = 0; i < 4; i++) 
		_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
	return _floatOut.value;
}

void write_EE_float(float value, int address)
{
	union {
		byte bytes[4];
		float value;
	} _floatIn;
	
	_floatIn.value = value;
	for (int i = 0; i < 4; i++) 
		eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
}

/********************************************************************************/

float
read_EE_compressed_float(int address, byte places)
{
	float scale = pow(10, places);
	
	int temp 	= eeprom_read_word((uint16_t *) address);
	return ((float)temp / scale);
}

void write_EE_compressed_float(float value, int address, byte places)
{
	float scale = pow(10, places);
	int temp 	= value * scale;
	eeprom_write_word((uint16_t *) 	address, temp);
}