#include <stdio.h>
#include <string.h>
#include "vecposition.h"

/*
 * Example code for parsing the distance and angle to the ball
 */

int main() {
  char buf[1000] = "(See (G2R (pol 9.25 -33.02 47.15)) (G1R (pol 9.22 -14.26 50.98)) (F1R (pol 10.95 25.61 44.45)) (F2R (pol 11.00 -58.35 28.64)) (B (pol 0.49 -12.96 -21.28)) (P (team assignemnt1) (id 3) (rlowerarm (pol 0.23 -35.67 -15.48)) (llowerarm (pol 0.23 35.20 -15.63)) (rfoot (pol 0.48 -11.59 -39.65)) (lfoot (pol 0.48 10.84 -39.93))))";


 char * str = strstr(buf, "(B (pol"); 
 float r = -1; 
 float t = -1;
 float p = -1;
 sscanf(str, "(B (pol %f %f %f)", &r, &t, &p); 

 printf("Polar offset = r:%f t:%f p%f\n", r, t, p); //for testing 

 VecPosition ballPolarOffset = VecPosition(r, t, p);

 // This is how we convert from polar to cartesian coordinates
 VecPosition ballCartesianOffset = ballPolarOffset.getCartesianFromPolar();

 // Account for the camera offset from the center of robot's body
 ballCartesianOffset = bodyModel->transformCameraToOrigin(ballCartesianOffset);
 
 ballCartesianOffset.setZ(0); // Zero out z element as we don't care about height
 cout << "Cartesian offset = " << ballCartesianOffset << endl; // for testing

 double dist = ballCartesianOffset.getMagnitude();
 double angle = ballCartesianOffset.getTheta();
 cout << "dist = " << dist << ", angle = " << angle << endl; 
 

}