/** \file
 * Focus control.
 *
 * Support focus stacking and other focus controls.
 * \todo Figure out how to really tell if a focus event is over.  The
 * property PROP_LV_FOCUS_DONE doesn't seem to really indicate that it
 * is safe to send another one.
 */
#include "dryos.h"
#include "menu.h"
#include "bmp.h"
#include "lens.h"
#include "config.h"
#include "ptp.h"


CONFIG_INT( "focus.step",	focus_stack_step, 100 );
CONFIG_INT( "focus.count",	focus_stack_count, 5 );

CONFIG_INT( "focus.follow", follow_focus, 1 );
CONFIG_INT( "focus.follow.rev.h", follow_focus_reverse_h, 0); // for left/right buttons
CONFIG_INT( "focus.follow.rev.v", follow_focus_reverse_v, 0); // for up/down buttons

static int focus_dir;
int get_focus_dir() { return focus_dir; }
int is_follow_focus_active() { return follow_focus; }
int get_follow_focus_stop_on_focus() { return follow_focus == 2; }
int get_follow_focus_dir_v() { return follow_focus_reverse_v ? -1 : 1; }
int get_follow_focus_dir_h() { return follow_focus_reverse_h ? -1 : 1; }

#define FOCUS_MAX 1700
//~ static int focus_position;

static struct semaphore * focus_stack_sem;




static void
focus_stack_unlock( void * priv )
{
	gui_stop_menu();
	give_semaphore( focus_stack_sem );
}


static void
display_lens_hyperfocal(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	unsigned		font = FONT_MED;
	unsigned		height = fontspec_height( font );

	bmp_printf( font, x, y,
		"Focal dist: %s",
		lens_info.focus_dist == 0xFFFF
                        ? " Infnty"
                        : lens_format_dist( lens_info.focus_dist * 10 )
	);

	y += height;
	bmp_printf( font, x, y,
		"Hyperfocal: %s",
		lens_format_dist( lens_info.hyperfocal )
	);

	y += height;
	bmp_printf( font, x, y,
		"DOF Near:   %s",
		lens_format_dist( lens_info.dof_near )
	);

	y += height;
	bmp_printf( font, x, y,
		"DOF Far:    %s",
		lens_info.dof_far >= 1000*1000
			? " Infnty"
			: lens_format_dist( lens_info.dof_far )
	);
}


void focus_stack_ensure_preconditions()
{
	while (lens_info.job_state) msleep(100);
	if (!lv_drawn())
	{
		get_out_of_play_mode();
		while (!lv_drawn())
		{
			bmp_printf(FONT_LARGE, 10, 30, "Please switch to LiveView");
			msleep(100);
		}
		msleep(200);
	}

	while (is_manual_focus())
	{
		bmp_printf(FONT_LARGE, 10, 30, "Please enable autofocus");
		msleep(100);
	}
}

void
focus_stack(
	unsigned		count,
	int			step
)
{
	bmp_printf( FONT_LARGE, 10, 30, "Focus stack: %dx%d", count, step );
	hdr_create_script(count, 0, 1);
	msleep(1000);
	
	int focus_moved_total = 0;

	unsigned i;
	for( i=0 ; i < count ; i++ )
	{
		if (gui_menu_shown()) break;
		
		bmp_printf( FONT_LARGE, 10, 30, "Focus stack: %d of %d", i+1, count );
		msleep( 500 );
		
		focus_stack_ensure_preconditions();
		lens_take_picture( 64 );
		
		if( count-1 == i )
			break;
		
		focus_stack_ensure_preconditions();

		lens_focus( 1, step );
		//~ lens_focus_wait();
		focus_moved_total += step;
	}

	msleep(1000);
	bmp_printf( FONT_LARGE, 10, 30, "Focus stack done!         " );

	// Restore to the starting focus position
	focus_stack_ensure_preconditions();
	lens_focus( 0, -focus_moved_total );
}


static void
focus_stack_task( void* unused )
{
	while(1)
	{
		take_semaphore( focus_stack_sem, 0 );
		msleep( 100 );
		focus_stack( focus_stack_count, focus_stack_step );
	}
}

TASK_CREATE( "fstack_task", focus_stack_task, 0, 0x1f, 0x1000 );

static struct semaphore * focus_task_sem;
static int focus_task_dir;
static int focus_task_delta;
static int focus_rack_delta;
CONFIG_INT( "focus.rack-speed", focus_rack_speed, 100 );
CONFIG_INT( "focus.delay", lens_focus_delay, 85 );

void follow_focus_reverse_dir()
{
	focus_task_dir = -focus_task_dir;
}

void plot_focus_status()
{
	if (gui_menu_shown()) return;
	bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 160, "%s        ", focus_task_dir > 0 ? "FAR " : focus_task_dir < 0 ? "NEAR" : "    ");
}

static void
focus_dir_display( 
	void *			priv,
	int			x,
	int			y,
	int			selected
) {

	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Focus dir     : %s",
		focus_dir ? "FAR " : "NEAR"
	);
	menu_draw_icon(x, y, MNI_ON, 0);
}

static void
focus_show_a( 
	void *			priv,
	int			x,
	int			y,
	int			selected
) {

	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Focus A       : %d",
		focus_task_delta
	);
	menu_draw_icon(x, y, MNI_ACTION, 0);
}


static void
focus_reset_a( void * priv )
{
	focus_task_delta = 0;
}


static void
focus_toggle( void * priv )
{
	focus_task_delta = -focus_task_delta;
	focus_rack_delta = focus_task_delta;
	give_semaphore( focus_task_sem );
}

int rack_speed_values[] = {1,2,3,4,5,7,10,15,20,25,30,40,50,60,75,85,100,150,200,300,500,1000};

static void
focus_rack_speed_display(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Focus speed   : %d",
		focus_rack_speed
	);
	menu_draw_icon(x, y, MNI_PERCENT, current_speed_index(focus_rack_speed) * 100 / COUNT(rack_speed_values));
}


int current_speed_index(int speed)
{
	int i;
	for (i = 0; i < COUNT(rack_speed_values); i++)
		if (speed == rack_speed_values[i]) return i;
	return 0;
}

static void
focus_rack_speed_increment( void * priv )
{
	int i = current_speed_index(focus_rack_speed);
	focus_rack_speed = rack_speed_values[mod(i + 1, COUNT(rack_speed_values))];
}

static void
focus_rack_speed_decrement( void * priv )
{
	int i = current_speed_index(focus_rack_speed);
	focus_rack_speed = rack_speed_values[mod(i - 1, COUNT(rack_speed_values))];
}

static void
focus_delay_display(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Focus delay   : %d",
		lens_focus_delay
	);
	menu_draw_icon(x, y, MNI_PERCENT, current_speed_index(lens_focus_delay) * 100 / COUNT(rack_speed_values));
}

static void
focus_stack_step_increment( void * priv )
{
	int i = current_speed_index(focus_stack_step);
	focus_stack_step = rack_speed_values[mod(i + 1, COUNT(rack_speed_values))];
}

static void
focus_stack_count_increment( void * priv )
{
	focus_stack_count = mod(focus_stack_count + 1, 100);
	if (focus_stack_count < 2) focus_stack_count = 2;
}

static void
focus_delay_toggle( int sign)
{
	int i = current_speed_index(lens_focus_delay);
	lens_focus_delay = rack_speed_values[mod(i + sign, COUNT(rack_speed_values))];
}
static void focus_delay_increment(void* priv) { focus_delay_toggle(1); }
static void focus_delay_decrement(void* priv) { focus_delay_toggle(-1); }

static void
focus_stack_print(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Stack focus   : %dx%d",
		focus_stack_count, focus_stack_step
	);
	bmp_printf(FONT_MED, x + 450, y-1, "PLAY: Run\nSET/Q: Adjust");
	menu_draw_icon(x, y, MNI_ACTION, 0);
}

void
lens_focus_start(
	int		dir
)
{
	if( dir == 0 )
		focus_task_dir = focus_dir ? 1 : -1;
	else
		focus_task_dir = dir;

	give_semaphore( focus_task_sem );
}


void
lens_focus_stop( void )
{
	focus_task_dir = 0;
}

static void
rack_focus(
	int		speed,
	int		delta
)
{
	DebugMsg( DM_MAGIC, 3,
		"%s: speed=%d delta=%d",
		__func__,
		speed,
		delta
	);

	if( speed <= 0 )
		speed = 1;

	int		speed_cmd = speed;

	// If we are moving closer, invert the speed command
	if( delta < 0 )
	{
		speed_cmd = -speed;
		delta = -delta;
	}

	while( delta )
	{
		if( speed > delta )
			speed = delta;

		delta -= speed;
		lens_focus( 0x7, speed_cmd );
		msleep(lens_focus_delay);
	}
}


static void
focus_task( void* unused )
{
	while(1)
	{
		msleep(50);
		take_semaphore( focus_task_sem, 0 );

		if( focus_rack_delta )
		{
			gui_hide_menu( 10 );
			rack_focus(
				focus_rack_speed,
				focus_rack_delta
			);

			focus_rack_delta = 0;
			continue;
		}

		while( focus_task_dir )
		{
			int step = focus_task_dir * focus_rack_speed;
			lens_focus( 1, step );
			msleep(lens_focus_delay);
			focus_task_delta += step;
		}
	}
}

TASK_CREATE( "focus_task", focus_task, 0, 0x1d, 0x1000 );


//~ PROP_HANDLER( PROP_LV_FOCUS )
//~ {
	//~ return prop_cleanup( token, property );
	//~ static int16_t oldstep = 0;
	//~ const struct prop_focus * const focus = (void*) buf;
	//~ const int16_t step = (focus->step_hi << 8) | focus->step_lo;
	//~ bmp_printf( FONT_SMALL, 200, 30,
		//~ "FOCUS: %08x active=%02x dir=%+5d (%04x) mode=%02x",
			//~ *(unsigned*)buf,
			//~ focus->active,
			//~ (int) step,
			//~ (unsigned) step & 0xFFFF,
			//~ focus->mode
		//~ );
	//~ return prop_cleanup( token, property );
//~ }

static void
follow_focus_toggle_dir_h( void * priv )
{
	follow_focus_reverse_h = !follow_focus_reverse_h;
}
static void
follow_focus_toggle_dir_v( void * priv )
{
	follow_focus_reverse_v = !follow_focus_reverse_v;
}

static void
follow_focus_print(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Follow Focus  : %s",
		follow_focus ? "ON" : "OFF"
	);
	if (follow_focus)
	{
		bmp_printf(FONT_MED, x + 480, y+5, follow_focus_reverse_h ? "- +" : "+ -");
		bmp_printf(FONT_MED, x + 480 + font_med.width, y-4, follow_focus_reverse_v ? "-\n+" : "+\n-");
	}
	menu_draw_icon(x, y, MNI_BOOL_LV(follow_focus), 0);
}

CONFIG_INT("movie.af", movie_af, 0);
#ifdef CONFIG_MOVIE_AF
CONFIG_INT("movie.af.aggressiveness", movie_af_aggressiveness, 4);
CONFIG_INT("movie.af.noisefilter", movie_af_noisefilter, 7); // 0 ... 9
int movie_af_stepsize = 10;
#endif


int focus_value = 0; // heuristic from 0 to 100
int focus_value_delta = 0;

volatile int focus_done = 0;
volatile uint32_t focus_done_raw = 0;

PROP_HANDLER(PROP_LV_FOCUS_DONE)
{
        focus_done_raw = buf[0];
        focus_done = 1;
        return prop_cleanup(token, property);
}


int get_focus_graph() 
{ 
	if (movie_af || get_follow_focus_stop_on_focus())
		return !is_manual_focus() && zebra_should_run();

	if (get_trap_focus() && can_lv_trap_focus_be_active())
		return zebra_should_run() || get_halfshutter_pressed();

	return 0;
}

int lv_focus_confirmation = 0;
static int hsp_countdown = 0;
int can_lv_trap_focus_be_active()
{
	if (!lv_drawn()) return 0;
	if (hsp_countdown) return 0; // half-shutter can be mistaken for DOF preview, but DOF preview property triggers a bit later
	if (dofpreview) return 0;
	if (shooting_mode == SHOOTMODE_MOVIE) return 0;
	if (gui_state != GUISTATE_IDLE) return 0;
	if (get_silent_pic_mode()) return 0;
	if (!is_manual_focus()) return 0;
	return 1;
}

static int hsp = 0;
int movie_af_reverse_dir_request = 0;
PROP_HANDLER(PROP_HALF_SHUTTER)
{
	if (buf[0] && !hsp) movie_af_reverse_dir_request = 1;
	hsp = buf[0];
	hsp_countdown = 3;
	if (get_zoom_overlay_z()) zoom_overlay_set_countdown(0);
	
	return prop_cleanup(token, property);
}


int get_lv_focus_confirmation() 
{ 
	if (!can_lv_trap_focus_be_active()) return 0;
	if (!get_halfshutter_pressed()) return 0;
	int ans = lv_focus_confirmation;
	lv_focus_confirmation = 0;
	return ans; 
}

int is_manual_focus()
{
	return (af_mode & 0xF) == 3;
}

#ifdef CONFIG_MOVIE_AF
int movie_af_active()
{
	return shooting_mode == SHOOTMODE_MOVIE && lv_drawn() && !is_manual_focus() && (focus_done || movie_af==3);
}

static void movie_af_step(int mag)
{
	if (!movie_af_active()) return;
	
	#define MAXSTEPSIZE 64
	#define NP ((int)movie_af_noisefilter)
	#define NQ (10 - NP)
	
	int dirchange = 0;
	static int dir = 1;
	static int prev_mag = 0;
	static int target_focus_rate = 1;
	if (mag == prev_mag) return;
	
	bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "    ");

	static int dmag = 0;
	dmag = ((mag - prev_mag) * NQ + dmag * NP) / 10; // focus derivative is filtered (it's noisy)
	int dmagp = dmag * 10000 / prev_mag;
	static int dmagp_acc = 0;
	static int acc_num = 0;
	dmagp_acc += dmagp;
	acc_num++;
	
	if (focus_done_raw & 0x1000) // bam! focus motor has hit something
	{
		dirchange = 1;
		bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "BAM!");
	}
	else if (movie_af_reverse_dir_request)
	{
		dirchange = 1;
		movie_af_reverse_dir_request = 0;
		bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "REV ");
	}
	else
	{
		if (dmagp_acc < -500 && acc_num >= 2) dirchange = 1;
		if (ABS(dmagp_acc) < 500)
		{
			bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, " !! "); // confused
		}
		else
		{
			dmagp_acc = 0;
			acc_num = 0;
			bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, " :) "); // it knows exactly if it's going well or not
		}

		if (ABS(dmagp) > target_focus_rate) movie_af_stepsize /= 2;       // adjust step size in order to maintain a preset rate of change in focus amount
		else movie_af_stepsize = movie_af_stepsize * 3 / 2;               // when focus is "locked", derivative of focus amount is very high => step size will be very low
		movie_af_stepsize = COERCE(movie_af_stepsize, 2, MAXSTEPSIZE);    // when OOF, derivative is very small => will increase focus speed
	}
	
	if (dirchange)
	{
		dir = -dir;
		dmag = 0;
		target_focus_rate /= 4;
	}
	else
	{
		target_focus_rate = target_focus_rate * 11/10;
	}
	target_focus_rate = COERCE(target_focus_rate, movie_af_aggressiveness * 20, movie_af_aggressiveness * 100);

	focus_done = 0;	
	static int focus_pos = 0;
	int focus_delta = movie_af_stepsize * SGN(dir);
	focus_pos += focus_delta;
	lens_focus(7, focus_delta);  // send focus command

	//~ bmp_draw_rect(7, COERCE(350 + focus_pos, 100, 620), COERCE(380 - mag/200, 100, 380), 2, 2);
	
	if (get_global_draw())
	{
		bmp_fill(0, 8, 151, 128, 10);                                          // display focus info
		bmp_fill(COLOR_RED, 8, 151, movie_af_stepsize, 5);
		bmp_fill(COLOR_BLUE, 8, 156, 64 * target_focus_rate / movie_af_aggressiveness / 100, 5);
		bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 160, "%s %d%%   ", dir > 0 ? "FAR " : "NEAR", dmagp/100);
	}
	prev_mag = mag;
}
#endif

#define NMAGS 64
static int mags[NMAGS] = {0};
#define FH COERCE(mags[i] * 45 / maxmagf, 0, 50)
int maxmagf = 1;

static void update_focus_mag(int mag)
{
	int maxmag = 1;
	int i;
	#define WEIGHT(i) (i > 40 ? 1 : 0.2)
	for (i = 0; i < NMAGS-1; i++)
		if (mags[i] * WEIGHT(i) > maxmag) maxmag = mags[i] * WEIGHT(i);
	maxmagf = (maxmagf * 4 + maxmag * 1) / 5;
	
	for (i = 0; i < NMAGS-1; i++)
		mags[i] = mags[i+1];
	mags[i] = mag;

	focus_value_delta = FH * 2 - focus_value;
	focus_value = FH * 2;
	lv_focus_confirmation = (focus_value + focus_value_delta*3 > 110);
}
static void plot_focus_mag()
{
	if (gui_state != GUISTATE_IDLE) return;
	if (!lv_drawn()) return;
	if (!get_global_draw()) return;
	
	BMP_SEM(
		int i;
		for (i = 0; i < NMAGS-1; i++)
		{
			bmp_draw_rect(COLOR_BLACK, 8 + i, 100, 0, 50);
			bmp_draw_rect(COLOR_YELLOW, 8 + i, 150 - FH, 0, FH);
		}
		//~ ff_check_autolock();
	)
}
#undef FH
#undef NMAGS

/*
void ff_check_autolock()
{
	static int rev_countdown = 0;
	static int stop_countdown = 0;
	if (is_follow_focus_active())
	{
		if (get_global_draw())
		{
			plot_focus_status();
			bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "    ");
		}
		if (get_follow_focus_stop_on_focus() && !stop_countdown)
		{
			if (!rev_countdown)
			{
				if (focus_value - focus_value_delta*5 > 110)
				{
					follow_focus_reverse_dir();
					rev_countdown = 5;
					bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "PEAK");
				}
			}
			else
			{
				rev_countdown--;
				bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "PEAK");
				if (focus_value + focus_value_delta*5 > 110) rev_countdown = 0;
				if (!rev_countdown)
				{
					bmp_printf(FONT(FONT_MED, COLOR_WHITE, 0), 30, 180, "LOCK");
					lens_focus_stop();
					stop_countdown = 10;
				}
			}
		}
		if (stop_countdown) stop_countdown--;
	}
}*/

int focus_mag_a = 0;
int focus_mag_b = 0;
int focus_mag_c = 0;
PROP_HANDLER(PROP_LV_FOCUS_DATA)
{
	focus_mag_a = buf[2];
	focus_mag_b = buf[3];
	focus_mag_c = buf[4];
	
#ifdef CONFIG_MOVIE_AF
	if (movie_af != 3)
#endif
	{
		update_focus_mag(focus_mag_a + focus_mag_b);
		if (get_focus_graph()) plot_focus_mag();
#ifdef CONFIG_MOVIE_AF
		if ((movie_af == 2) || (movie_af == 1 && get_halfshutter_pressed())) 
			movie_af_step(focus_mag_a + focus_mag_b);
#endif
	}
	return prop_cleanup(token, property);
}

#ifdef CONFIG_MOVIE_AF
static void
movie_af_print(
	void *			priv,
	int			x,
	int			y,
	int			selected
)
{
	if (movie_af)
		bmp_printf(
			selected ? MENU_FONT_SEL : MENU_FONT,
			x, y,
			"Movie AF      : %s A%d N%d",
			movie_af == 1 ? "Hold" : movie_af == 2 ? "Cont" : movie_af == 3 ? "CFPk" : "Err",
			movie_af_aggressiveness,
			movie_af_noisefilter
		);
	else
		bmp_printf(
			selected ? MENU_FONT_SEL : MENU_FONT,
			x, y,
			"Movie AF      : OFF"
		);
}

void movie_af_aggressiveness_bump(void* priv)
{
	movie_af_aggressiveness = movie_af_aggressiveness * 2;
	if (movie_af_aggressiveness > 64) movie_af_aggressiveness = 1;
}
void movie_af_noisefilter_bump(void* priv)
{
	movie_af_noisefilter = (movie_af_noisefilter + 1) % 10;
}
#endif

static void
focus_misc_task(void* unused)
{
	while(1)
	{
		msleep(100);
		
		if (hsp_countdown) hsp_countdown--;

#ifdef CONFIG_MOVIE_AF
		if (movie_af == 3)
		{
			int fm = get_spot_focus(100);
			update_focus_mag(fm);
			if (get_focus_graph()) plot_focus_mag();
			movie_af_step(fm);
		}
#endif
	}
}

TASK_CREATE( "focus_misc_task", focus_misc_task, 0, 0x1e, 0x1000 );

static void 
trap_focus_display( void * priv, int x, int y, int selected )
{
	int t = (*(int*)priv);
	bmp_printf(
		selected ? MENU_FONT_SEL : MENU_FONT,
		x, y,
		"Trap Focus    : %s",
		t == 1 ? "Hold" : t == 2 ? "Cont." : "OFF"
	);
}


extern int trap_focus;


static struct menu_entry focus_menu[] = {
	{
		.priv		= &trap_focus,
		.select		= menu_binary_toggle,
		.display	= trap_focus_display,
		.help = "Takes a picture when the subject comes in focus. MF only."
	},
	{
		.priv = &follow_focus,
		.display	= follow_focus_print,
		.select		= menu_binary_toggle,
		.select_reverse = follow_focus_toggle_dir_v,
		.select_auto = follow_focus_toggle_dir_h,
		.help = "Simple follow focus with arrow keys."
	},
#ifdef CONFIG_MOVIE_AF
	{
		.priv = &movie_af,
		.display	= movie_af_print,
		.select		= menu_quaternary_toggle,
		.select_reverse = movie_af_noisefilter_bump,
		.select_auto = movie_af_aggressiveness_bump,
		.help = "Custom AF algorithm in movie mode. Not very efficient."
	},
#endif
	{
		.display	= focus_rack_speed_display,
		.select		= focus_rack_speed_increment,
		.select_reverse	= focus_rack_speed_decrement,
		.help = "Speed for rack focus and follow focus, in raw steps."
	},
	{
		.display	= focus_delay_display,
		.select		= focus_delay_increment,
		.select_reverse	= focus_delay_decrement,
		.help = "Delay between two successive focus commands."
	},
	{
		.priv		= &focus_dir,
		.display	= focus_dir_display,
		.select		= menu_binary_toggle,
		.help = "Focus direction used when you half-press the shutter."
	},
	{
		.display	= focus_show_a,
		.select		= focus_reset_a,
		.help = "Press SET to fix here the end point of rack focus."
	},
	{
		.priv		= "Rack focus",
		.display	= menu_print,
		.select		= focus_toggle,
		.help = "Start the rack focus operation."
	},
	{
		.display	= focus_stack_print,
		.select		= focus_stack_count_increment,
		.select_auto		= focus_stack_step_increment,
		.select_reverse		= focus_stack_unlock,
		.help = "Focus bracketing, useful for macro shots."
	},
	{
		.display	= display_lens_hyperfocal,
	},
};




static void
focus_init( void* unused )
{
	focus_stack_sem = create_named_semaphore( "focus_stack_sem", 0 );
	focus_task_sem = create_named_semaphore( "focus_task_sem", 1 );

	menu_add( "Focus", focus_menu, COUNT(focus_menu) );
}

/*
PTP_HANDLER( 0x9998, 0 )
{
	int step = (int) param1;

	focus_position += step;
	if( focus_position < 0 )
		focus_position = 0;
	else
	if( focus_position > FOCUS_MAX )
		focus_position = FOCUS_MAX;

	lens_focus( 0x7, (int) param1 );
	bmp_printf( FONT_MED, 650, 35, "%04d", focus_position );

	struct ptp_msg msg = {
		.id		= PTP_RC_OK,
		.session	= session,
		.transaction	= transaction,
		.param_count	= 2,
		.param		= { param1, focus_position },
	};

	context->send(
		context->handle,
		&msg
	);

	return 0;
}
*/

INIT_FUNC( __FILE__, focus_init );