#include <draw.h>
#include <calc.h>

#ifdef __AVR
#include <avr/pgmspace.h>
#include <tal/usart_funcs.h>
#else // !__AVR
#include <stdio.h>
#include <string.h>
#define PROGMEM
#define pgm_read_ptr(x) (*x)
#define memcpy_P memcpy
#endif

#define CLEAN_COUNT 2

#ifdef __AVR
void send_info_double( double v, uint8_t c1, uint8_t c2 );
#endif

#ifdef __AVR
#define ACCEL(x) (x)
#else
#define ACCEL(x) (10*x)
#endif

#ifdef __AVR
#define DBG(x) while(0)
#else
#define DBG(x) x
#endif

namespace {

	// shape actions
	enum {
		SA_LINE = 1,
		SA_CIRCLE_LEFT,
		SA_CIRCLE_RIGHT,
		SA_MOVE,
		SA_UP,
		SA_DOWN,
		SA_END = 0
	};

	// structure for basic shapes
	struct Shape {
		uint8_t action;
		union {
			struct {
				uint8_t x;
				uint8_t y;
			} coord;
			struct {
				uint8_t r;
				uint8_t a1;
				uint8_t a2;
			} circle;
		} u;
	};

#if 0 // small zero
	const Shape shape_0[] PROGMEM = {
		{ SA_MOVE, { .coord={ 100, 50 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 0/15, 0/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
#else // tall zero
	const Shape shape_0[] PROGMEM = {
		{ SA_MOVE, { .coord={ 100, 50 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 100, 150 } } },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 0/15, 180/15 } } },
		{ SA_LINE, { .coord={ 0, 50 } } },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 180/15, 0/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
#endif
	const Shape shape_1[] PROGMEM = {
		{ SA_MOVE, { .coord={ 0, 100 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_LEFT, { .circle={ 100, 270/15, 0/15 } } },
		{ SA_LINE, { .coord={ 100, 0 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_2[] PROGMEM = {
		{ SA_MOVE, { .coord={ 0, 150 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_RIGHT, { .circle={ 50, 180/15, 0/15 } } },
		{ SA_LINE, { .coord={ 0, 0 } } },
		{ SA_LINE, { .coord={ 100, 0 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_3[] PROGMEM = {
		{ SA_MOVE, { .coord={ 0, 150 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_RIGHT, { .circle={ 50, 180/15, 270/15 } } },
		{ SA_CIRCLE_RIGHT, { .circle={ 50, 90/15, 180/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_4[] PROGMEM = {
		{ SA_MOVE, { .coord={ 75, 0 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 75, 200 } } },
		{ SA_LINE, { .coord={ 0, 60 } } },
		{ SA_LINE, { .coord={ 100, 60 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_5[] PROGMEM = {
		{ SA_MOVE, { .coord={ 100, 200 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 0, 200 } } },
		{ SA_LINE, { .coord={ 0, 100 } } },
		{ SA_LINE, { .coord={ 50, 100 } } },
		{ SA_CIRCLE_RIGHT, { .circle={ 50, 90/15, 180/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_6[] PROGMEM = {
		{ SA_MOVE, { .coord={ 75, 200 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 7, 75 } } },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 150/15, 150/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_7[] PROGMEM = {
		{ SA_MOVE, { .coord={ 0, 200 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 100, 200 } } },
		{ SA_LINE, { .coord={ 25, 0 } } },
		{ SA_UP, },
		{ SA_MOVE, { .coord={ 25, 100 } } },
		{ SA_DOWN, },
		{ SA_LINE, { .coord={ 75, 100 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_8[] PROGMEM = {
		{ SA_MOVE, { .coord={ 50, 200 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 90/15, 270/15 } } },
		{ SA_CIRCLE_RIGHT, { .circle={ 50, 90/15, 90/15 } } },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 270/15, 90/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_9[] PROGMEM = {
		{ SA_MOVE, { .coord={ 93, 125 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_LEFT, { .circle={ 50, 330/15, 330/15 } } },
		{ SA_LINE, { .coord={ 25, 0 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_colon[] PROGMEM = {
		{ SA_MOVE, { .coord={ 50, 150 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_RIGHT, { .circle={ 10, 270/15, 270/15 } } },
		{ SA_UP, },
		{ SA_MOVE, { .coord={ 50, 50 } } },
		{ SA_DOWN, },
		{ SA_CIRCLE_LEFT, { .circle={ 10, 90/15, 90/15 } } },
		{ SA_UP, },
		{ SA_END, }
	};
	const Shape shape_clean[] PROGMEM = {
		{ SA_END, }
	};
	const Shape* const shapes[] PROGMEM = {
		shape_0,
		shape_1,
		shape_2,
		shape_3,
		shape_4,
		shape_5,
		shape_6,
		shape_7,
		shape_8,
		shape_9,
		shape_colon,
		shape_clean
	};

	enum {
		ST_STOP = 0,
		ST_PARKING_1,
		ST_PARKING_2,
		ST_CLEANING,
		ST_STARTING_1,
		ST_STARTING_2,
		ST_CHAR_1
	};

	enum {
		MOVE_NONE = 0,
		MOVE_LINE,
		MOVE_CIRCLE,
		MOVE_PEN_DOWN
	};

#ifndef __AVR
	const char* state_name( int x )
	{
		switch( x )
		{
			case ST_STOP: return "ST_STOP";
			case ST_PARKING_1: return "ST_PARKING_1";
			case ST_PARKING_2: return "ST_PARKING_2";
			case ST_CLEANING: return "ST_CLEANING";
			case ST_STARTING_1: return "ST_STARTING_1";
			case ST_STARTING_2: return "ST_STARTING_2";
			case ST_CHAR_1: return "ST_CHAR_1";
			case ST_CHAR_1+1: return "ST_CHAR_2";
			case ST_CHAR_1+2: return "ST_CHAR_3";
			case ST_CHAR_1+3: return "ST_CHAR_4";
			case ST_CHAR_1+4: return "ST_CHAR_5";
			case ST_CHAR_1+5: return "ST_CHAR_6";
			default: return "ST_?????";
		}
	}

	const char* action_name( int x )
	{
		switch( x )
		{
			case SA_LINE: return "SA_LINE";
			case SA_CIRCLE_LEFT: return "SA_CIRCLE_LEFT";
			case SA_CIRCLE_RIGHT: return "SA_CIRCLE_RIGHT";
			case SA_MOVE: return "SA_MOVE";
			case SA_UP: return "SA_UP";
			case SA_DOWN: return "SA_DOWN";
			case SA_END: return "SA_END";
			default: return "SA_??????";
		}
	}

	const char* move_name( int x )
	{
		switch( x )
		{
			case MOVE_NONE: return "MOVE_NONE";
			case MOVE_LINE: return "MOVE_LINE";
			case MOVE_CIRCLE: return "MOVE_CIRCLE";
			case MOVE_PEN_DOWN: return "MOVE_PEN_DOWN";
			default: return "MOVE_??????";
		}
	}
#endif // ! __AVR

};



Draw Draw::singleton;

void Draw::init()
{
	start_writing = 0;
	state = ST_STOP;
	moving = MOVE_NONE;
	x = Params::params.parking_x;
	y = Params::params.parking_y;
}

void Draw::draw( uint8_t hours, uint8_t minutes, Servos& servos )
{
#ifdef __AVR__
	if( delay > 0 )
	{
		delay--;
		return;
	}
#endif // __AVR__
	if( moving == MOVE_NONE )
	{
		switch( state )
		{
			case ST_STOP:
				// stopped, wait for start
				if( start_writing )
				{
					start_writing = 0;
					shapenrs[0] = hours / 10;
					shapenrs[1] = hours - 10 * shapenrs[0];
					shapenrs[2] = 10;
					shapenrs[3] = minutes / 10;
					shapenrs[4] = minutes - 10 * shapenrs[3];
					state = ST_CLEANING;
					index = 0;
					moving = MOVE_NONE;
#ifdef __AVR
					usart_add_byte( 't' );
					usart_add_byte( '=' );
					usart_add_byte( '0'+shapenrs[0] );
					usart_add_byte( '0'+shapenrs[1] );
					usart_add_byte( ':' );
					usart_add_byte( '0'+shapenrs[3] );
					usart_add_byte( '0'+shapenrs[4] );
					usart_add_byte( '\r' );
					usart_add_byte( '\n' );
#endif
				}
				break;
			case ST_PARKING_1:
				// move pen to parking position
				servos.lift_servo = Params::params.c.lift_up;
				x = Params::params.parking_x;
				y = Params::params.parking_y;
				state = ST_PARKING_2;
				delay = 50;
				break;
			case ST_PARKING_2:
				// put pen into cap
				servos.lift_servo = Params::params.c.lift_med;
				state = ST_STOP;
				delay = 20;
				break;
			case ST_CLEANING:
				// clean the area
				if( moving == MOVE_NONE )
				{
					calc_cleaning();
					// done here, do the delay
					goto draw_done;
				}
				break;
			case ST_STARTING_1:
				// start writing: lift the pen from the cap
				servos.lift_servo = Params::params.c.lift_up;
				delay = 20;
				state = ST_STARTING_2;
				break;
			case ST_STARTING_2:
				// initialize writing the first char
				state = ST_CHAR_1;
				calc_offset();
				x = offset_x;
				y = Params::params.middle_y;
				delay = 50;
				index = 0;
				break;
			default:
				// writing, when finished start parking the pen
				if( state >= (ST_CHAR_1 + 5) )
				{
					state = ST_PARKING_1;
				}
				else
				{
					next_shape( servos );
				}
				break;
		}
	}
	if( moving == MOVE_LINE )
	{
		// line
		move.line.factor += ACCEL( move.line.factor_inc );
#ifdef __AVR
		// send_info_double( move.line.factor, 'L', 'f' );
#endif
		if( move.line.factor >= 1.0 )
		{
			move.line.factor = 1.0;
			moving = MOVE_NONE;
		}
		x = move.line.start_x + move.line.factor * move.line.diff_x;
		y = move.line.start_y + move.line.factor * move.line.diff_y;
		delay = 0;
	}
	else if( moving == MOVE_CIRCLE )
	{
		// circle
		move.circle.a_cur += ACCEL( move.circle.a_inc );
#ifdef __AVR
		// send_info_double( move.circle.a_cur, 'L', 'f' );
#endif
		if( move.circle.a_inc > 0.0 )
		{
			if( move.circle.a_cur >= move.circle.a_end )
			{
				move.circle.a_cur = move.circle.a_end;
				moving = MOVE_NONE;
			}
		}
		else
		{
			if( move.circle.a_cur <= move.circle.a_end )
			{
				move.circle.a_cur = move.circle.a_end;
				moving = MOVE_NONE;
			}
		}
		x = move.circle.m_x + move.circle.r_x * cos(move.circle.a_cur);
		y = move.circle.m_y + move.circle.r_y * sin(move.circle.a_cur);
		delay = 0;
	}
	else if( moving == MOVE_PEN_DOWN )
	{
		if( servos.lift_servo > Params::params.c.lift_down )
		{
			servos.lift_servo -= 2;
			if( servos.lift_servo <= Params::params.c.lift_down )
			{
				servos.lift_servo = Params::params.c.lift_down;
				moving = MOVE_NONE;
				delay = 5;
			}
		}
		else
		{
			servos.lift_servo += 2;
			if( servos.lift_servo >= Params::params.c.lift_down )
			{
				servos.lift_servo = Params::params.c.lift_down;
				moving = MOVE_NONE;
				delay = 5;
			}
		}
	}
	else if( moving == MOVE_NONE )
	{
	}
	else
	{
		// invalid move
		moving = MOVE_NONE;
	}
	Params::params.calc( x, y, servos.left_servo, servos.right_servo );

draw_done:
#ifdef __AVR
	/*
	usart_add_byte( 'S' );
	usart_add_hexbyte( state );
	usart_add_byte( ' ' );
	usart_add_byte( 'M' );
	usart_add_hexbyte( moving );
	usart_add_byte( ' ' );
	usart_add_byte( 'i' );
	usart_add_hexbyte( index );
	usart_add_byte( '\r' );
	usart_add_byte( '\n' );
	send_info_double( x, 'M', 'x' );
	send_info_double( y, 'M', 'y' );
	delay = 50;
	*/
#else
	{
	static double prevx = 0.0, prevy = 0.0;
	double tmp, len;
	tmp = x - prevx;
	len = tmp * tmp;
	tmp = y - prevy;
	len = sqrt( len + tmp * tmp );
	printf( "state %s moving %s index %d x=%g y=%g (l=%g) dly=%d\n",
			state_name(state), move_name(moving), index, x, y, len, delay );
	prevx = x;
	prevy = y;
	}
#endif // __AVR
	return;
}

void Draw::calc_cleaning()
{
	double endx, endy;

	if( index > (CLEAN_COUNT + 1) )
	{
		state = ST_STARTING_1;
		index = 0;
		x = Params::params.parking_x;
		y = Params::params.parking_y;
		delay = 5;
		return;
	}
	else if( index == (CLEAN_COUNT + 1) )
	{
		endx = Params::params.parking_x;
		endy = Params::params.parking_y;
	}
	else
	{
		endy = Params::params.middle_y + 
			(index - (0.5*CLEAN_COUNT)) * Params::params.size_y * (1./CLEAN_COUNT);
		if( index & 1 )
		{
			endx = Params::params.middle_x + 2.5 * Params::params.size_x;
		}
		else
		{
			endx = Params::params.middle_x - 2.5 * Params::params.size_x;
		}
	}
	move.line.start_x = x;
	move.line.start_y = y;
	move.line.diff_x = endx - x;
	move.line.diff_y = endy - y;
	move.line.factor = 0.0;
	// move.line.factor_inc = 0.05;
	move.line.factor_inc = Params::params.size_y * 0.045 /
			sqrt( move.line.diff_x * move.line.diff_x +
					move.line.diff_y * move.line.diff_y );
	moving = MOVE_LINE;
	index++;
	// give the servos some time to reach their position
	delay = 5;
}

#define r2d(x) (x*180.0/3.1415926535)

void Draw::next_shape( Servos& servos )
{
	Shape shape;

	{
	uint8_t shapenr = shapenrs[state - ST_CHAR_1];
	const Shape* pshape = (const Shape*)pgm_read_ptr( &shapes[shapenr] );
	memcpy_P( &shape, pshape + index, sizeof(shape) );
	// printf( "shapenr=%d a=%d\n", shapenr, shapes[shapenr][index].action );
	}
	index++;
	DBG( printf( "shape.action = %s\n", action_name(shape.action) ) );
	switch( shape.action )
	{
		case SA_LINE:
			DBG( printf( "line %d/%d\n",
					shape.u.coord.x, shape.u.coord.y ) );
			move.line.start_x = x;
			move.line.start_y = y;
			move.line.diff_x = offset_x +
						shape.u.coord.x * Params::params.size_x * 0.01 - x;
			move.line.diff_y = offset_y +
						shape.u.coord.y * Params::params.size_y * 0.005 - y;
			move.line.factor = 0.0;
			move.line.factor_inc = Params::params.size_y * 0.015 /
					sqrt( move.line.diff_x * move.line.diff_x +
							move.line.diff_y * move.line.diff_y );
			moving = MOVE_LINE;
			DBG( printf( "  move.line.start_x    = %g\n", move.line.start_x ) );
			DBG( printf( "  move.line.start_y    = %g\n", move.line.start_y ) );
			DBG( printf( "  move.line.diff_x     = %g\n", move.line.diff_x ) );
			DBG( printf( "  move.line.diff_y     = %g\n", move.line.diff_y ) );
			DBG( printf( "  move.line.factor     = %g\n", move.line.factor ) );
			DBG( printf( "  move.line.factor_inc = %g\n", move.line.factor_inc ) );
#ifdef ZZ__AVR
		send_info_double( offset_x, 'x', 'o' );
		send_info_double( offset_y, 'y', 'o' );
		send_info_double( move.line.start_x, 'x', 's' );
		send_info_double( move.line.start_y, 'y', 's' );
		send_info_double( move.line.diff_x, 'x', 'd' );
		send_info_double( move.line.diff_y, 'y', 'd' );
		send_info_double( move.line.factor, 'f', 'f' );
		send_info_double( move.line.factor_inc, 'f', 'i' );
#endif
			break;
		case SA_CIRCLE_LEFT:
		case SA_CIRCLE_RIGHT:
			// r a1 a2
			{
			DBG( printf( "circle %d %d-%d\n",
					shape.u.circle.r, shape.u.circle.a1, shape.u.circle.a2 ) );
			double a1 = (M_PI/12.0) * shape.u.circle.a1;
			double a2 = (M_PI/12.0) * shape.u.circle.a2;
			if( shape.action == SA_CIRCLE_LEFT )
			{
				if( shape.u.circle.a2 <= shape.u.circle.a1 )
				{
					a2 += 2.0*M_PI;
				}
			}
			else
			{
				if( shape.u.circle.a2 >= shape.u.circle.a1 )
				{
					a2 -= 2.0*M_PI;
				}
			}
			move.circle.r_x = Params::params.size_x * 0.01 * shape.u.circle.r;
			move.circle.r_y = Params::params.size_y * 0.005 * shape.u.circle.r;
			move.circle.m_x = x - cos(a1) * move.circle.r_x;
			move.circle.m_y = y - sin(a1) * move.circle.r_y;
			move.circle.a_cur = a1;
			move.circle.a_end = a2;
			if( shape.action == SA_CIRCLE_LEFT )
			{
				move.circle.a_inc = M_PI / shape.u.circle.r;
			}
			else
			{
				move.circle.a_inc = -M_PI / shape.u.circle.r;
			}
			moving = MOVE_CIRCLE;
			DBG( printf( "  move.circle.r_x   = %g\n", move.circle.r_x ) );
			DBG( printf( "  move.circle.r_y   = %g\n", move.circle.r_y ) );
			DBG( printf( "  move.circle.m_x   = %g\n", move.circle.m_x ) );
			DBG( printf( "  move.circle.m_y   = %g\n", move.circle.m_y ) );
			DBG( printf( "  move.circle.a_cur = %g (%g)\n",
					move.circle.a_cur, r2d(move.circle.a_cur) ) );
			DBG( printf( "  move.circle.a_end = %g (%g)\n",
					move.circle.a_end, r2d(move.circle.a_end) ) );
			DBG( printf( "  move.circle.a_inc = %g (%g)\n",
					move.circle.a_inc, r2d(move.circle.a_inc) ) );
			}
			break;
		case SA_MOVE:
			DBG( printf( "move %d/%d\n",
					shape.u.coord.x, shape.u.coord.y ) );
			x = offset_x + shape.u.coord.x * Params::params.size_x * 0.01;
			y = offset_y + shape.u.coord.y * Params::params.size_y * 0.005;
			delay = 50;
			break;
		case SA_UP:
			DBG( printf( "pen up\n" ) );
			servos.lift_servo = Params::params.c.lift_med;
			delay = 20;
			break;
		case SA_DOWN:
			DBG( printf( "pen down\n" ) );
			moving = MOVE_PEN_DOWN;
			delay = 0;
			break;
		// case SA_END:
		default:
			state++;
			index = 0;
			calc_offset();
			break;
	}
}

void Draw::calc_offset()
{
	double fx;
#if 0
	switch( state )
	{
		case ST_CHAR_1:
			fx = -2.5;
			break;
		case ST_CHAR_1+1:
			fx = -1.4;
			break;
		case ST_CHAR_1+2:
			fx = -0.5;
			break;
		case ST_CHAR_1+3:
			fx = +0.4;
			break;
		default:
			fx = +1.5;
			break;
	}
#elif 0
	switch( state )
	{
		case ST_CHAR_1:
			fx = -3.5;
			break;
		case ST_CHAR_1+1:
			fx = -1.907;
			break;
		case ST_CHAR_1+2:
			fx = -0.714;
			break;
		case ST_CHAR_1+3:
			fx = +0.479;
			break;
		default:
			fx = +2.07;
			break;
	}
#else
	switch( state )
	{
		case ST_CHAR_1:
			fx = -3.0;
			break;
		case ST_CHAR_1+1:
			fx = -1.50;
			break;
		case ST_CHAR_1+2:
			fx = -0.5;
			break;
		case ST_CHAR_1+3:
			fx = +0.50;
			break;
		default:
			fx = +2.00;
			break;
	}
#endif
	offset_x = Params::params.middle_x + fx * Params::params.size_x;
	offset_y = Params::params.middle_y - 0.5 * Params::params.size_y;
#ifdef ZZ__AVR
	usart_add_byte( '1'-ST_CHAR_1+state );
	usart_add_byte( ' ' );
	send_info_double( fx, 'f', 'x' );
	send_info_double( offset_x, 'o', 'x' );
#endif
}

bool Draw::stopped()
{
	return state == ST_STOP;
}