/*
 * 3pi_sense_test.c
 *
 * Some simple tests of the IR sensors on the Pololu 3pi robot.
 *    http://www.pololu.com/catalog/product/975
 *
 * Copyright (c) George Eggleston
 * You may freely modify and share this code, as long as you keep this
 * notice intact.  Licensed under the Creative Commons BY-SA 3.0 license:
 *
 * http://creativecommons.org/licenses/by-sa/3.0/
 * 
 */
 
#include <pololu/3pi.h>
#include <avr/pgmspace.h>

// Menu text etc
const char main_menu1[] PROGMEM = "Main Menu";
const char main_menu2[] PROGMEM = "C  Im  M";
const char count_menu1[] PROGMEM = "Count M";
const char count_menu2[] PROGMEM = "S  KS  L";
const char sense_mode_menu1[] PROGMEM = "SMo Menu";
const char sense_mon1[] PROGMEM = "M Sen ";
const char welcome_music[] PROGMEM = "cge";

unsigned char ir_mode = IR_EMITTERS_ON;

// The number of sensor reads to do when test rates
const unsigned long num_counts = 2500;
	
void initialize(void);
void cal_sen(void);
unsigned char main_menu(void);
unsigned char count_menu(void);
void bat_test(void);
void count_sense(void);
void count_k_sense(void);
void count_l_sense(void);
void sense_mode_menu(void);
void sense_mon(void);

// set things up and then loop forever responding to input
int main()
{
    unsigned char mm_return, cm_return;
	initialize();
	for(;;)
	{
		mm_return = main_menu();
		switch(mm_return)
        {
        case BUTTON_A:
            cm_return = count_menu();
            switch(cm_return)
            {
                case BUTTON_A:
                    count_sense();
                    break;
                case BUTTON_B:
                    count_k_sense();
                    break;
                case BUTTON_C:
                    count_l_sense();
                    break;
            }
            break;
        case BUTTON_B:
            wait_for_button_release(ALL_BUTTONS);
            sense_mode_menu();
            break;
        case BUTTON_C:
            wait_for_button_release(ALL_BUTTONS);
            sense_mon();
            break;
        }
        delay_ms(125);
        bat_test();
        delay_ms(1200);
	}
}
// Initialize 3pi and offer to calibrate sensors
void initialize()
{
    unsigned char cal_but;
    clear();
	print("  INIT   ");
	pololu_3pi_init(2000);
	play_from_program_space(welcome_music);
	while(is_playing());
	lcd_goto_xy(0,1);
	print("Cal (b)");
	cal_but = wait_for_button_press(ALL_BUTTONS);
    delay_ms(500);
    if(cal_but == BUTTON_B)
    {
    	cal_sen();
	}
}
// Calibrate sensors
void cal_sen()
{
	clear();
	print("Cal Sen");
	for(int counter=0;counter<80;counter++)
	{
		if(counter < 20 || counter >= 60)
		{
			set_motors(40,-40);
		}
		else
		{
			set_motors(-40,40);
		}
		calibrate_line_sensors(IR_EMITTERS_ON);
		calibrate_line_sensors(IR_EMITTERS_OFF);
		delay_ms(20);
	}
	set_motors(0,0);
}
// Display main menu and wait for input
unsigned char main_menu()
{
    unsigned char mm_choice;
	clear();
	print_from_program_space(main_menu1);
	lcd_goto_xy(0,1);
	print_from_program_space(main_menu2);
	mm_choice = wait_for_button(ALL_BUTTONS);
	return(mm_choice);
}
// Displays the battery voltage.
void bat_test()
{
	int bat = read_battery_millivolts();
	clear();
	print_long(bat);
	print(" mV");

	delay_ms(100);
}
// Display menu to choose what type of measurement to count cycles on
unsigned char count_menu()
{
    unsigned char cm_choice;
	clear();
	print_from_program_space(count_menu1);
	lcd_goto_xy(0,1);
	print_from_program_space(count_menu2);
	cm_choice = wait_for_button(ALL_BUTTONS);
	return(cm_choice);
}
// Count cycles on uncalibrated read
void count_sense()
{
	unsigned int sensor_val[5];
	long start_time;
	
	clear();
	print("S Only nc");
	lcd_goto_xy(0,1);
	
	start_time = get_ms();
	for (int i = 0; i < num_counts; i++)
	{
		read_line_sensors(sensor_val, ir_mode);
	}

	print_long((num_counts * 1000) / (get_ms() - start_time));

	wait_for_button_press(ALL_BUTTONS);
	wait_for_button_release(ALL_BUTTONS);
	delay_ms(500);
}
// Count cycles on calibrated read
void count_k_sense()
{
	unsigned int sensor_val[5];
	long start_time;
	
	clear();
	print("S Only K");
	lcd_goto_xy(0,1);
	
	start_time = get_ms();
	for (int i = 0; i < num_counts; i++)
	{
		read_line_sensors_calibrated(sensor_val, ir_mode);
	}

	print_long((num_counts * 1000) / (get_ms() - start_time));

	wait_for_button_press(ALL_BUTTONS);
	wait_for_button_release(ALL_BUTTONS);
	delay_ms(500);
}
// Count cycles on computed(line) read
void count_l_sense()
{
	unsigned int sensor_val[5];
	long start_time;
	
	clear();
	print("L Mode");
	lcd_goto_xy(0,1);
	
	start_time = get_ms();
	for (int i = 0; i < num_counts; i++)
	{
		read_line(sensor_val, ir_mode);
	}

	print_long((num_counts * 1000) / (get_ms() - start_time));

	wait_for_button_press(ALL_BUTTONS);
	wait_for_button_release(ALL_BUTTONS);
	delay_ms(500);
}
// Read and or set mode of emitters on reads
void sense_mode_menu()
{
    unsigned char sm_choice;
    clear();
    print_from_program_space(sense_mode_menu1);
	lcd_goto_xy(0,1);
	switch(ir_mode)
    {
        case IR_EMITTERS_ON:
            print("I+ (c)");
            break;
        case IR_EMITTERS_OFF:
            print("I- (c)");
            break;
        case IR_EMITTERS_ON_AND_OFF:
            print("Ib (c)");
            break;
    }
    sm_choice = wait_for_button(ALL_BUTTONS);
    wait_for_button_release(ALL_BUTTONS);
    if(sm_choice == BUTTON_C)
    {
        sm_choice = wait_for_button(ALL_BUTTONS);
        wait_for_button_release(ALL_BUTTONS);
        switch(sm_choice)
        {
            case BUTTON_A:
                ir_mode = IR_EMITTERS_ON;
                break;
            case BUTTON_B:
                ir_mode = IR_EMITTERS_OFF;
                break;
            case BUTTON_C:
                ir_mode = IR_EMITTERS_ON_AND_OFF;
                break;
        }
        delay(500);
    }
}
// Read center sensor using Pololu line function
void sense_mon()
{
	unsigned int sensor_val[5];
	clear();
	print_from_program_space(sense_mon1);
	switch(ir_mode)
    {
        case IR_EMITTERS_ON:
            print("+");
            break;
        case IR_EMITTERS_OFF:
            print("-");
            break;
        case IR_EMITTERS_ON_AND_OFF:
            print("b");
            break;
    }
	
	for(;;)
	{
		read_line_sensors(sensor_val, ir_mode);
		lcd_goto_xy(0,1);
		print("        ");
		lcd_goto_xy(0,1);
		print_long(sensor_val[2]);
		for(int i = 0; i < 75; i++)
		{
    		delay_ms(10);
    		if(button_is_pressed(ALL_BUTTONS))
    		{
    		    wait_for_button_press(ALL_BUTTONS);
    		    return;
    		}
		}
	}

}