// Test code
// PICO drives 1 meter forward, 1 meter backward, 1 meter left, 1 meter right, 180 degree turn left, 180 degree turn right.
// PICO should end in its initial position.

#include <emc/io.h>
#include <emc/rate.h>
#include <cmath>
#include <iostream>

emc::IO io; // Create IO object, which will initialize the io layer
emc::OdometryData odom;
emc::LaserData scan;
emc::Rate r(10);

float xvec, yvec;

bool forcevector()
{
	float rpico 0.1;
	float Fpush -100;

	float da = 4/scan.ranges.size();
	float a = -2;

	float cosvec;
	float sinvec;
	float totvec;

	for(int i = 0; i < scan.ranges.size(); i++)
	{
		cosvec += cos(a)/pow(fabs(scan.ranges[i]-rpico),3);
		sinvec += sin(a)/pow(fabs(scan.ranges[i]-rpico),3);
		totvec += 1     /pow(fabs(scan.ranges[i]-rpico),3);
		a      += da;
	}

	xvec = (Fpush*cosvec)/totvec;
	yvec = (Fpush*sinvec)/totvec;

	return true;
}


int main()
{
	float picomass = 20;
	float time = 0.1;

	float ax;
	float ay;
	float vx;
	float vy;

	while(io.ok())
	{
		forcevector();
		ax = xvec*picomass;
		ay = yvec*picomass;

		vx = ax*time;
		vy = ay*time;

		io.sendBaseReference(vx,vy,0);
		r.sleep();
	}
}