#include <iostream>
#include <iomanip>

#include "wiimote.h"
#include "Vect3D.h"

#define _USE_MATH_DEFINES
#include <math.h>

#define USING_WIIYOURSELF
#include "C_3DPointSmoother.h"

#define SPACES "                      "
#define BLANKLINE "                                                                      "

using namespace std;

int main (int argc, char **argv)
{
  HANDLE console = GetStdHandle(STD_OUTPUT_HANDLE);
  COORD  origin = {0, 0};

  Vect3D_t l_Dot[2];
  C_3DPointSmoother l_smoother0, l_smoother1;

  //l_smoother0.SetExponentialMovingAverage(0.2);
  //l_smoother1.SetExponentialMovingAverage(0.2);
  l_smoother0.SetSimpleMovingAverage(10);
  l_smoother1.SetSimpleMovingAverage(10);

  wiimote l_wm = wiimote();
  l_wm.ChangedCallback = NULL; //no callbacks, we just poll...
	l_wm.CallbackTriggerFlags = NO_CHANGE;

  cout << "Connecting";
  while (!l_wm.Connect())
  {
    cout << ".";
    Sleep(500);
  }
  cout << "\nConnected!\n";

  //enable ir 
  l_wm.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true);

  Sleep(100);

  cout.fill(' ');
  cout.setf(ios::left, ios::adjustfield);

  do
  {
    l_wm.RefreshState();

    SetConsoleCursorPosition(console, origin);

    if (FindSensorBarDots(l_wm.IR, l_Dot))
    {
      l_Dot[0] = l_smoother0.Smooth(l_Dot[0]);
      l_Dot[1] = l_smoother1.Smooth(l_Dot[1]);
      cout << "Smoothed Dots:" << endl;
      cout << setw(32) << l_Dot[0] ;
      cout << setw(32) << l_Dot[1] << endl << endl;

      cout << setw(32) << "Dot distance:" << endl;
      Vect3D_t dif = l_Dot[0] - l_Dot[1];
      double dist2 = dif.x*dif.x + dif.y*dif.y + dif.z*dif.z;
      double dist = sqrt(dist2);
      cout << setw(32) << dist << endl << endl;


      double radPerPixel = ((M_PI / 180)*40.0)/1016.0,
             angle = dist * radPerPixel,
             z = (0.5 * 205.0) / tan(0.5 * angle); 
      cout << setw(32) << "Relative coordinates:" << endl;
      cout << "x (px): " << setw(10) << fixed << setprecision(2) 
           << (l_Dot[0].x + l_Dot[1].x)/2.0 << endl; 
      cout << "y (px): " << setw(10) << fixed << setprecision(2) 
           << (l_Dot[0].y + l_Dot[1].y)/2.0 << endl; 
      cout << "z (mm): " << setw(10) << fixed << setprecision(2) << z << endl;


    }
    else
    {
      cout << "no dots...";

      for (int i=0 ; i<20; i++)
        cout << BLANKLINE << endl;
    }

    Sleep(10);
    
  }
  while (!l_wm.Button.Home());//!l_wm.Button.Home());

  cout << "Exiting...\n";

  //l_wm.Disconnect();


  return 0;
}