/*    
     ____________________________________________________
    |                                                   |
    |                                                   |
    |                                                   |
    |                                                   |
    |                    |            |                 |
    |                    |            |                 |
    |                    |            |                 |
    |                    |            |                 |
    |                    |____________|                 |
    |                                                   |
    |                                                   |
    |                                                   |
    |                                                   |
    |                                                   |
    |____________________     y       __________________|
                              ^
                              |
                              |    
                              |------> x
                              (0,0) Theta Right hand rule
*/

// STANDARD ANSI INCLUDES
#include <std.h> // DSP/BIOS standard include file
#include <hwi.h>
#include <swi.h>
#include <log.h> // LOG_printf calls
#include <mem.h> // MEM_alloc calls
#include <que.h> // QUE functions
#include <sem.h> // Semaphore functions
#include <sys.h>
#include <tsk.h> // TASK functions
#include <rtdx.h> // RTDX functions
#include <math.h> // sinf,cosf,fabsf, etc.
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

// DSP INCLUDES
#include <c6x.h>         // register defines, in c:\ti\c6000\cgtools\include
#include <c6x11dsk.h>    // TI header in the directory c:\ti\c6000\dsk6x11\include
#include <fastrts67x.h>  // TI's real-time math library, in c:\ti\c6700\mthlib\include

// COECSL INCLUDES       // all COECSL functions are usually in the directory f:\C6713DSK\include
#include <c6xdskdigio.h> // COECSL functions for daugher card, encs, PWM
#include <max3100uart.h> // COECSL functions for communication to max serial chip
#include <dac7724.h>     // COECSL functions for the DAC7724 chip
#include <ad7864.h>     // COECSL functions for the AD7864 chip
#include <RCservo.h>     // COECSL functions to set up PWM ch3 and ch4 to drive RC servos
#include <switch_led.h>  // COECSL functions for turning off LEDs, monitoring switches
#include <dspvisioncolor50Hz_cLCD.h>
#include <i2c.h>
#include <edma.h>
#include <sharpir.h>
#include <dsk6713.h>
#include <user_ColorVisionFuncs.h>
#include <user_UARTFuncs.h>
#include <user_PIFuncs.h>
#include <user_IR_UltraFuncs.h>
#include <atmel_pwrboard2.h>
#include <color_LCD.h>

#include "xy.h"

float my_atanf(float dy, float dx)
{
	float ang;
    
	if (fabsf(dy) <= 0.001F) {
		if (dx >= 0.0F) {
			ang = 0.0F;
		} else {
			ang = PI;
		}
	} else if (fabsf(dx) <= 0.001F) {
		if (dy > 0.0F) {
			ang = HALFPI;
		} else {
			ang = -HALFPI;
		}
	} else {
		ang = atan2f(dy,dx);
	}
	return ang;
}











int xy_control(float *vref_forxy, float *turn_forxy,float turn_thres, float x_pos,float y_pos,float x_desired,float y_desired,
                float thetaabs,float target_radius,float target_radius_near)
{
	float dx,dy,alpha;
	float dist = 0.0F;
	float dir;
	float theta;
	int target_near = FALSE;
    float turnerror = 0;

		// calculate theta (current heading) between -PI and PI
	if (thetaabs > PI) {
		theta = thetaabs - 2.0*PI*floorf((thetaabs+PI)/(2.0*PI));
	} else if (thetaabs < -PI) {
		theta = thetaabs - 2.0*PI*ceilf((thetaabs-PI)/(2.0*PI));
	} else {
		theta = thetaabs;
	}

	dx = x_desired - x_pos;
	dy = y_desired - y_pos;
	dist = sqrtf( dx*dx + dy*dy );
	dir = 1.0F;

	// calculate alpha (trajectory angle) between -PI and PI
	alpha = my_atanf(dy,dx);

	// calculate turn error
	//	turnerror = theta - alpha;  old way using left hand coordinate system.
	turnerror = alpha - theta;

	// check for shortest path
	if (fabsf(turnerror + 2.0*PI) < fabsf(turnerror)) turnerror += 2.0*PI;
	else if (fabsf(turnerror - 2.0*PI) < fabsf(turnerror)) turnerror -= 2.0*PI;

	if (dist < target_radius_near) {
		target_near = TRUE;
		// Arrived to the target's (X,Y)
		if (dist < target_radius) {
			dir = 0.0F;
			turnerror = 0.0F;
		} else {
			// if we overshot target, we must change direction. This can cause the robot to bounce back and forth when 
            // remaining at a point.
			if (fabsf(turnerror) > HALFPI) {
				dir = -dir;
			}            
			turnerror = 0;
		}
	} else {
		target_near = FALSE;
	}

	// vref is 1 tile/sec; but slower when close to target.  
	*vref_forxy = dir*MIN(dist,1);

    if (fabsf(*vref_forxy) > 0.0) {
        // if robot 1 tile away from target use a scaled KP value.  
        *turn_forxy = (*vref_forxy*2)*turnerror;
    } else {
        // normally use a Kp gain of 2
        *turn_forxy = 2*turnerror;
    }
    
    // This helps with unbalanced wheel slipping.  If turn value greater than 
    // turn_thres then just spin in place
	if (fabsf(*turn_forxy) > turn_thres) {
		*vref_forxy = 0;
	}
	return(target_near);
}