/*
 www.ArduCopter.com - www.DIYDrones.com
 Copyright (c) 2010.  All rights reserved.
 An Open Source Arduino based multicopter.
 
 File     : Log.pde
 Version  : v1.0, Aug 27, 2010
 Author(s): ArduCopter Team
             Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
             Jani Hirvinen, Ken McEwans, Roberto Navoni,          
             Sandro Benigno, Chris Anderson
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.

* ************************************************************** *
ChangeLog:


* ************************************************************** *
TODO:


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


// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs

#define HEAD_BYTE1 0xA3    // Decimal 163
#define HEAD_BYTE2 0x95    // Decimal 149
#define END_BYTE 0xBA      // Decimal 186

#define LOG_ATTITUDE_MSG		0x01
#define LOG_GPS_MSG			0x02
#define LOG_MODE_MSG			0X03
#define LOG_CONTROL_TUNING_MSG	        0X04
#define LOG_NAV_TUNING_MSG		0X05
#define LOG_PERFORMANCE_MSG		0X06
#define LOG_RAW_MSG			0x07
#define LOG_RADIO_MSG                   0x08
#define LOG_SENSOR_MSG                  0x09
#define LOG_PID_MSG                     0x0A


//  Function to display available logs and solicit action from the user.
//  --------------------------------------------------------------------
void Print_Log_Menu(void)
{
	int log_start;
	int log_end;
	eeprom_busy_wait();
	byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);			eeprom_busy_wait();	
	if (last_log_num == 0) {
		Serial.println("No logs available for download");
	} else {
		Serial.print(last_log_num,DEC);
		Serial.println(" logs available for download");
		for(int i=1;i<last_log_num+1;i++) {
			log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i-1)*0x02));			eeprom_busy_wait();	
			log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i)*0x02))-1;			eeprom_busy_wait();	
			if (i == last_log_num) {	
				log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE);			eeprom_busy_wait();	
			}	
			Serial.print("Log number ");
			Serial.print(i);
			Serial.print(",    start page ");
			Serial.print(log_start);
			Serial.print(",   end page ");
			Serial.println(log_end);
		}
	}
	Serial.println(" ");
	Serial.println("Input 1 <enter> to dump a log");
	Serial.println("Input 2 <enter> to erase all logs");
	Serial.println("Input 3 <enter> to leave log mode");
	Serial.println(" ");
}
	
	
//  Function to get desired action from the user.
//  ---------------------------------------------
byte Get_User_Log_Command(void)
{

	byte step=0;
	byte char_1;
	byte char_2;
	byte char_3;
	
	Serial.flush();
	
	while(step<3)
	{
		if(Serial.available()){
			char_1 = Serial.read();
			char_2 = Serial.read();
			char_3 = Serial.read();

			if (char_1<0x30 || char_1>0x39) {
				return 0;
			} else if (char_2 == 0xFF) {
				return (char_1 - 0x30);
  			} else if (char_2<0x30 || char_2>0x39) {
				return 0;
			} else if (char_2 != 0xFF) {
				return 0;
			} else {
				return ((char_1 - 0x30)*10 + char_2 - 0x30);
			}

		} else {
			delay(20);
		}
	}
}
	


// Write an attitude packet. Total length : 10 bytes
#if LOG_ATTITUDE
void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw)
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_ATTITUDE_MSG);
	DataFlash.WriteInt(log_roll);
	DataFlash.WriteInt(log_pitch);
	DataFlash.WriteInt(log_yaw);
	DataFlash.WriteByte(END_BYTE);
}
#endif


#ifdef LOG_SEN
// Write a Sensor Raw data packet
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7)
{
  DataFlash.WriteByte(HEAD_BYTE1);
  DataFlash.WriteByte(HEAD_BYTE2);
  DataFlash.WriteByte(LOG_SENSOR_MSG);
  DataFlash.WriteInt(s1);
  DataFlash.WriteInt(s2);
  DataFlash.WriteInt(s3);
  DataFlash.WriteInt(s4);
  DataFlash.WriteInt(s5);
  DataFlash.WriteInt(s6);
  DataFlash.WriteInt(s7);
  DataFlash.WriteByte(END_BYTE);
}
#endif

// Write a PID control info
void Log_Write_PID(byte num_PID, int P, int I,int D, int output)
{
  DataFlash.WriteByte(HEAD_BYTE1);
  DataFlash.WriteByte(HEAD_BYTE2);
  DataFlash.WriteByte(LOG_PID_MSG);
  DataFlash.WriteByte(num_PID);
  DataFlash.WriteInt(P);
  DataFlash.WriteInt(I);
  DataFlash.WriteInt(D);
  DataFlash.WriteInt(output);
  DataFlash.WriteByte(END_BYTE);
}

// Write a Radio packet
void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
{
  DataFlash.WriteByte(HEAD_BYTE1);
  DataFlash.WriteByte(HEAD_BYTE2);
  DataFlash.WriteByte(LOG_RADIO_MSG);
  DataFlash.WriteInt(ch1);
  DataFlash.WriteInt(ch2);
  DataFlash.WriteInt(ch3);
  DataFlash.WriteInt(ch4);
  DataFlash.WriteInt(ch5);
  DataFlash.WriteInt(ch6);
  DataFlash.WriteByte(END_BYTE);
}

#if LOG_PM
// Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance()
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
	DataFlash.WriteLong(millis() - perf_mon_timer);
	DataFlash.WriteInt(mainLoop_count);
	DataFlash.WriteInt(G_Dt_max);
	DataFlash.WriteByte(gyro_sat_count);
	DataFlash.WriteByte(adc_constraints);
	DataFlash.WriteByte(renorm_sqrt_count);
	DataFlash.WriteByte(renorm_blowup_count);
	DataFlash.WriteByte(gps_fix_count);
	DataFlash.WriteInt((int)(imu_health*1000));
	DataFlash.WriteByte(END_BYTE);
}
#endif

#if LOG_CTUN
// Write a control tuning packet. Total length : 22 bytes
void Log_Write_Control_Tuning()
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
	DataFlash.WriteInt((int)(servo_out[CH_ROLL]*100));
	DataFlash.WriteInt((int)nav_roll);
	DataFlash.WriteInt((int)roll_sensor);
	DataFlash.WriteInt((int)(servo_out[CH_PITCH]*100));
	DataFlash.WriteInt((int)nav_pitch);
	DataFlash.WriteInt((int)pitch_sensor);
	DataFlash.WriteInt((int)(servo_out[CH_THROTTLE]*100));
	DataFlash.WriteInt((int)(servo_out[CH_RUDDER]*100));
	DataFlash.WriteInt((int)AN[5]);
	DataFlash.WriteByte(END_BYTE);
}
#endif

#if LOG_NTUN
// Write a navigation tuning packet. Total length : 18 bytes
void Log_Write_Nav_Tuning()
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
	DataFlash.WriteInt((int)yaw_sensor);
	DataFlash.WriteInt((int)wp_distance);
	DataFlash.WriteInt((int)target_bearing);
	DataFlash.WriteInt((int)nav_bearing);
	DataFlash.WriteInt(altitude_error);
	DataFlash.WriteInt((int)airspeed_error);
	DataFlash.WriteInt((int)(nav_gain_scaler*1000));
	DataFlash.WriteByte(END_BYTE);
}
#endif

#if LOG_MODE
// Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode)
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_MODE_MSG);
	DataFlash.WriteByte(mode);
	DataFlash.WriteByte(END_BYTE);
}
#endif

#if LOG_GPS
// Write an GPS packet. Total length : 30 bytes
void Log_Write_GPS(	long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt, 
					long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_GPS_MSG);
	DataFlash.WriteLong(log_Time);
	DataFlash.WriteByte(log_Fix);
	DataFlash.WriteByte(log_NumSats);
	DataFlash.WriteLong(log_Lattitude);
	DataFlash.WriteLong(log_Longitude);
	DataFlash.WriteLong(log_mix_alt);
	DataFlash.WriteLong(log_gps_alt);
	DataFlash.WriteLong(log_Ground_Speed);
	DataFlash.WriteLong(log_Ground_Course);
	DataFlash.WriteByte(END_BYTE);
}
#endif

#if LOG_RAW
// Write an raw accel/gyro data packet. Total length : 28 bytes
void Log_Write_Raw()
{
	DataFlash.WriteByte(HEAD_BYTE1);
	DataFlash.WriteByte(HEAD_BYTE2);
	DataFlash.WriteByte(LOG_RAW_MSG);
	for(int i=0;i<6;i++) 
		DataFlash.WriteLong((long)(AN[i]*1000.0));
	DataFlash.WriteByte(END_BYTE);
}
#endif


// Read an control tuning packet
void Log_Read_Control_Tuning()
{	
	float logvar;

	Serial.print("CTUN:");
	for (int y=1;y<10;y++) {
		logvar = DataFlash.ReadInt();
		if(y < 9) logvar = logvar/100.f;
		Serial.print(logvar);
		Serial.print(",");
	}
	Serial.println(" ");
}

// Read a nav tuning packet
void Log_Read_Nav_Tuning()
{	
	float logvar;

	Serial.print("NTUN:");
	for (int y=1;y<8;y++) {
		logvar = DataFlash.ReadInt();
		if(y > 6) logvar = logvar/1000.f;
		Serial.print(logvar);
		Serial.print(",");
	}
	Serial.println(" ");
}

// Read a performance packet
void Log_Read_Performance()
{	
	long pm_time;
	int logvar;

	Serial.print("PM:");
	pm_time = DataFlash.ReadLong();
	Serial.print(pm_time);
	Serial.print(",");
	for (int y=1;y<9;y++) {
		if(y<3 || y>7) logvar = DataFlash.ReadInt();
		else logvar = DataFlash.ReadByte();
		if(y > 7) logvar = logvar/1000.f;
		Serial.print(logvar);
		Serial.print(",");
	}
	Serial.println(" ");
}

// Read an attitude packet
void Log_Read_Attitude()
{	
	int log_roll;
	int log_pitch;
	int log_yaw;

	log_roll = DataFlash.ReadInt();
	log_pitch = DataFlash.ReadInt();
	log_yaw = DataFlash.ReadInt(); 
	Serial.print("ATT:");
	Serial.print(log_roll);
	Serial.print(",");
	Serial.print(log_pitch);
	Serial.print(",");
	Serial.print(log_yaw);
	Serial.println();
}

// Read a Sensor raw data packet
void Log_Read_PID()
{  
  Serial.print("PID:");
  Serial.print((int)DataFlash.ReadByte());  // NUM_PID
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());  // P
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());  // I
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());  // D
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());  // output
  Serial.println();
}

// Read an Radio packet
void Log_Read_Radio()
{  
  Serial.print("RADIO:");
  Serial.print(DataFlash.ReadInt());
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());
  Serial.print(",");
  Serial.print(DataFlash.ReadInt());
  Serial.println();
}

// Read a mode packet
void Log_Read_Mode()
{	
	byte mode;

	mode = DataFlash.ReadByte();
	Serial.print("MOD:");
	switch (mode) {
		case 0:
			Serial.println("Manual mode");
			break;
		case 1:
			Serial.println("Stabilize mode");
			break;
		case 5:
			Serial.println("Fly By Wire A mode");
			break;
		case 6:
			Serial.println("Fly By Wire B mode");
			break;
		case 10:
			Serial.println("AUTO mode");
			break;
		case 11:
			Serial.println("RTL mode");
			break;
		case 12:
			Serial.println("Loiter mode");
			break;
		case 98:
			Serial.println("Air Start Complete");
			break;
		case 99:
			Serial.println("Ground Start Complete");
			break;
			}
}

// Read a GPS packet
void Log_Read_GPS()
{
	long log_Time;
	byte log_Fix;
	byte log_NumSats;
	long log_Lattitude;
	long log_Longitude;
	long log_gps_alt;
	long log_mix_alt;
	float log_Ground_Speed;
	float log_Ground_Course;
 
	log_Time = DataFlash.ReadLong();
	log_Fix = DataFlash.ReadByte();
	log_NumSats = DataFlash.ReadByte();
	log_Lattitude = DataFlash.ReadLong();
	log_Longitude = DataFlash.ReadLong();
	log_mix_alt = DataFlash.ReadLong();
	log_gps_alt = DataFlash.ReadLong();
	log_Ground_Speed = DataFlash.ReadLong();
	log_Ground_Course = DataFlash.ReadLong();

	Serial.print("GPS:");
	Serial.print(log_Time);
	Serial.print(",");
	Serial.print((int)log_Fix);
	Serial.print(",");
	Serial.print((int)log_NumSats);
	Serial.print(",");
	Serial.print(log_Lattitude);
	Serial.print(",");
	Serial.print(log_Longitude);
	Serial.print(",");
	Serial.print(log_mix_alt/100.0);
	Serial.print(",");
	Serial.print(log_gps_alt/100.0);
	Serial.print(",");
	Serial.print(log_Ground_Speed/100.0f);			
	Serial.print(",");
	Serial.print(log_Ground_Course/100.0);	 //	This is incorrect!!!!!
	Serial.println();

}

// Read a raw accel/gyro packet
void Log_Read_Raw()
{	
	float logvar;

	Serial.print("RAW:");
	for (int y=0;y<6;y++) {
		logvar = DataFlash.ReadLong()/1000.f;
		Serial.print(logvar);
		Serial.print(",");
	}
	Serial.println(" ");
}

// Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page)
{
	byte data;
	byte log_step=0;
	int packet_count=0; 
	int flash_led = 1;

	DataFlash.StartRead(start_page);
	while (DataFlash.GetPage() < end_page)
		{
		data = DataFlash.ReadByte();
		switch(log_step)		 //This is a state machine to read the packets
			{
			case 0:	
				if(data==HEAD_BYTE1)	// Head byte 1
					log_step++;
				break; 
			case 1:
				if(data==HEAD_BYTE2)	// Head byte 2
					log_step++;
				break; 
			case 2:
				if(data==LOG_ATTITUDE_MSG){
					Log_Read_Attitude();
					log_step++;
				
				}else if(data==LOG_MODE_MSG){
					Log_Read_Mode();
					log_step++;

				}else if(data==LOG_CONTROL_TUNING_MSG){
					Log_Read_Control_Tuning();
					log_step++;

				}else if(data==LOG_NAV_TUNING_MSG){
					Log_Read_Nav_Tuning();
					log_step++;
				
				}else if(data==LOG_PERFORMANCE_MSG){
					Log_Read_Performance();
					log_step++;
					
				}else if(data==LOG_RAW_MSG){
					Log_Read_Raw();
					log_step++;
                                }else if(data==LOG_PID_MSG){
					Log_Read_PID();
					log_step++;
				}else {
					if(data==LOG_GPS_MSG){
						Log_Read_GPS();
						log_step++;
					} else {
						Serial.print("Error Reading Packet: ");
						Serial.print(packet_count); 
						log_step=0;	 // Restart, we have a problem...
					}
				}
				break;
			case 3:
				if(data==END_BYTE){
					 packet_count++;
				}else{
					Serial.print("Error Reading END_BYTE  ");
					Serial.println(data,DEC);
				}
				log_step=0;			// Restart sequence: new packet...
				if(flash_led > 0) {
					digitalWrite(A_LED_PIN,HIGH);
					flash_led++;
					if(flash_led>100) flash_led=-1;
				} else {
					digitalWrite(A_LED_PIN,LOW);
					flash_led--;
					if(flash_led<-100) flash_led=1;
				}
				break;
			}
		}
	Serial.print("Number of packets read: ");
	Serial.println(packet_count);
	digitalWrite(A_LED_PIN,HIGH);
}