#include <calc.h>
#include <stdio.h>

void calc_and_print( double x, double y )
{
	uint16_t left, right;

	printf( "\nx:                  %g\n", x );
	printf( "y:                  %g\n", y );
	if( Params::params.calc( x, y, left, right ) )
	{
		printf( "left:               %d\n", left );
		printf( "right:              %d\n", right );
	}
}

int main( int argc, char** argv )
{
	calc_debug = true;
	/*
	Params::params.c.servo_distance = 23.0;
	Params::params.c.arm_length = 50.0;
	Params::params.c.elbow_pen_distance = 59.0;
	Params::params.c.hands_pen_distance = 11.0;
	Params::params.c.left_factor = 180.0/M_PI;
	Params::params.c.right_factor = 180.0/M_PI;
	Params::params.c.left_offset = 0.0;
	Params::params.c.right_offset = 0.0;
	Params::params.c.lift_up = 1;
	Params::params.c.lift_med = 2;
	Params::params.c.lift_down = 3;
	*/

	Params::params.load();
	Params::params.precalc();

	printf( "servo_distance:     %f\n", Params::params.c.servo_distance );
	printf( "arm_length:         %f\n", Params::params.c.arm_length );
	printf( "elbow_pen_distance: %f\n", Params::params.c.elbow_pen_distance );
	printf( "hands_pen_distance: %f\n", Params::params.c.hands_pen_distance );
	printf( "left_factor:        %f\n", Params::params.c.left_factor );
	printf( "right_factor:       %f\n", Params::params.c.right_factor );
	printf( "left_offset:        %f\n", Params::params.c.left_offset );
	printf( "right_offset:       %f\n", Params::params.c.right_offset );
	printf( "lift_up:            %d\n", Params::params.c.lift_up );
	printf( "lift_med:           %d\n", Params::params.c.lift_med );
	printf( "lift_down:          %d\n", Params::params.c.lift_down );
	printf( "checksum:           %02X\n", Params::params.c.checksum );
	printf( "fx:                 %f\n", Params::params.fx );
	printf( "fy:                 %f\n", Params::params.fy );
	printf( "arm_length_2neg:    %f\n", Params::params.arm_length_2neg );
	printf( "parking_x:          %f\n", Params::params.parking_x );
	printf( "parking_y:          %f\n", Params::params.parking_y );
	printf( "middle_x:           %f\n", Params::params.middle_x );
	printf( "middle_y:           %f\n", Params::params.middle_y );
	printf( "size_x:             %f\n", Params::params.size_x );
	printf( "size_y:             %f\n", Params::params.size_y );

	calc_and_print( 50.0, -50.0 );
	calc_and_print( 60.0, -50.0 );

	return 0;
}