#include <map>
#include <iostream>
#include <iomanip>

#include "typedefs.h"
#include "message_input.h"
#include "wiimote_utils.h"
#include "C_Smoother.h"


AbstractWiimote::AbstractWiimote()
  : m_prevButtons(0) 
{
  m_pSensBarDotSmoother[0] = new C_Smoother<Vect3D_t>(Vect3D_t(0.0, 0.0, 0.0));
  m_pSensBarDotSmoother[0]->SetExponentialMovingAverage(0.2);
  m_pSensBarDotSmoother[1] = new C_Smoother<Vect3D_t>(Vect3D_t(0.0, 0.0, 0.0));
  m_pSensBarDotSmoother[1]->SetExponentialMovingAverage(0.2);
}

AbstractWiimote::~AbstractWiimote()
{
  delete m_pSensBarDotSmoother[0];
  delete m_pSensBarDotSmoother[1];
}

void AbstractWiimote::FillWiimoteMsgPayload(input_payload_wiimote &f_payload, double f_dSensBarLedDist)
{
  Vect3D_t l_Dot[2];

  ParseWiimote(f_payload);
  
  //set the bttnsDown bitmap
  f_payload.btnsDown = (f_payload.btns ^ m_prevButtons) & ~m_prevButtons;
  //set the bttnsUp bitmap
  f_payload.btnsUp = (f_payload.btns ^ m_prevButtons) & m_prevButtons;

  m_prevButtons = f_payload.btns;

  f_payload.relX = f_payload.relY = f_payload.Zdist = 0.0;
  f_payload.posDataValid = CalcWiimoteRelativeCursorPos(f_payload, f_dSensBarLedDist);
}

double AbstractWiimote::CalcZDistInMM(Vect3D_t f_Dot[2], double f_dLedDist)
{
  double  l_dX = f_Dot[0].x - f_Dot[1].x,                   //difference in x coordinates
          l_dY = f_Dot[0].y - f_Dot[1].y,                   //difference in y coordinates
          l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY),         //distance between ir dots (in camera pixels)
          l_dDotAngle = g_dWiimoteRadPerPixel * l_dDotDist;  //the angle between the lines from the camera through the two 
                                                            //ir dots (in radians)

  return (0.5*f_dLedDist)/tan(0.5*l_dDotAngle); //the distance between the sensorbar and wiimote (in mm)
}


//define some struct rawdot operators
//inline const struct rawdot &operator=(struct rawdot &lhs, const struct rawdot &rhs) 
//{ lhs.rx=rhs.rx; lhs.ry=rhs.ry; return lhs;}
inline const struct rawdot &operator-=(struct rawdot &lhs, const struct rawdot &rhs) 
{ lhs.rx-=rhs.rx; lhs.ry-=rhs.ry; return lhs; }
inline const struct rawdot operator-(const struct rawdot &lhs, const struct rawdot &rhs) 
{ struct rawdot tmp = lhs; return tmp-=rhs;}

inline ostream& operator<<(ostream& os, const struct rawdot& r ) 
{
  stringstream ss;
  ss << setprecision(4)
     << "(" << r.rx << ", " << r.ry << ")";

  return os << ss.str();
}

bool AbstractWiimote::FindSensorBarDots(struct rawdot *f_rd, int f_iNumdots,  Vect3D_t f_Dot[2])
{
  //the ends of the sensorbar contain more 3 to 5 ir leds each. the wiimote
  //occaisonally detects individual ir leds at the same sensorbar end
  //the ir dots have to be mapped to the ends of the sensor bar, so when we
  //have more than 2 dots we have to group them into two groups
  int l_GroupId[4] = {-1, -1, -1, -1};

  if (f_iNumdots < 2)
  {
    //not enough dots
    return false;
  }
  else if (f_iNumdots == 2)
  {
    //two dots (easy case)
    l_GroupId[0] = 0;
    l_GroupId[1] = 1;
    //sort the dots below
  }
  else //more than 2 dots...
  {
    //group dots that are close to each other and compute their average position
    //first compute the squared distance of all dot pairs and then groupe them
    //into two groups based on their proximities
    typedef pair<int,int> intpair;
    map<int, intpair> m_Distances; //the squared distances between the pair of dots
    
    //iterate through all pairs and compute their squared distance
    for(int i=0; i<f_iNumdots; i++)
    {
      for(int j=i+1; j<f_iNumdots; j++)
      {
        rawdot d = f_rd[i] - f_rd[j];
        int dist2 = d.rx*d.rx + d.ry*d.ry;
        m_Distances[dist2] = intpair(i,j);
      }
    }

    //first assign the two dots that are furthest apart into two seperate groups then
    //iterate through the distances from smallest to largest and assign pairs of
    //dots closest to each other to a group. for 4 dots there are 6 pairs, for 3 dots 3 pairs
    //continue until all points are grouped into two groups 
    int l_iDotsLeft = f_iNumdots;         //number of dots not assigned to a group
    //get the pair of dots that are furthest away
    intpair l_dotpair = (--m_Distances.end())->second;
    l_GroupId[l_dotpair.first] = 0;
    l_GroupId[l_dotpair.second] = 1;
    l_iDotsLeft -= 2;

    for(map<int, intpair>::iterator it = m_Distances.begin(); 
      it != m_Distances.end() && l_iDotsLeft > 0; it++)
    {
      //check if the dots in the pair are assigned a group id
      intpair dots = it->second;

      //we can only add a dot to a group of the other dot in the pair is already
      //assigned a group id. If we find a pair of dots of which no dot is assigned 
      //a group id, then we ignore them. If the latter case occurs it means that 
      //the pair is closer to each other then the two initial dots, which means 
      //that we are probably not processing ir dots captured from the sensor bar
      if (l_GroupId[dots.first] != -1 && l_GroupId[dots.second] == -1)
      {
        //second belongs to the same group assigned to first
        l_GroupId[dots.second] = l_GroupId[dots.first];
        l_iDotsLeft--;
      }
      else if (l_GroupId[dots.first] == -1 && l_GroupId[dots.second] != -1)
      {
        //first belongs to the same group assigned to second
        l_GroupId[dots.first] = l_GroupId[dots.second];
        l_iDotsLeft--;
      }
    }
  }

  //calculate the average dots
  int l_iDotsPerGroup[2] = {0,0};

  f_Dot[0] = f_Dot[1] = Vect3D_t(0.0, 0.0, 0.0);
  for (int i=0; i<f_iNumdots; i++)
  {
    if (l_GroupId[i] > -1)
    {
      f_Dot[l_GroupId[i]] += Vect3D_t((double)f_rd[i].rx, (double)f_rd[i].ry, 0.0); 
      l_iDotsPerGroup[l_GroupId[i]]++;
    }
  }
  f_Dot[0] /= (double)l_iDotsPerGroup[0];
  f_Dot[1] /= (double)l_iDotsPerGroup[1];

  bool swapped = false;

  //sort the dots such that f_Dot[0] is the leftmost
  if (f_Dot[0].x > f_Dot[1].x || (f_Dot[0].x == f_Dot[1].x && f_Dot[0].y > f_Dot[1].y))
  {
    //swap
    Vect3D_t tmp = f_Dot[0];
    f_Dot[0] = f_Dot[1];
    f_Dot[1] = tmp;
    swapped = true;
  }

  ////print debug stuff
  /*if (f_iNumdots > 2)
  {
    int gid = swapped ? 1 : 0;

    cout.fill(' ');

    cout << setw(32) << "Group 0:" << "Group 1:" << endl;
    for (int i = 0; i < 4; i++)
    {
      if (l_GroupId[i] == gid && i < f_iNumdots)
      {
        cout << setw(32) << f_rd[i]  << setw(32) << "------" << endl;
      }
      else if (i < f_iNumdots)
      {
        cout << setw(32) << "------" << setw(32) << f_rd[i] << endl;
      }
      else
      {
        cout << setw(32) << "------" << setw(32) <<  "------" << endl;
      }
    }
    cout << endl << setw(32) << "Average:" << endl;
    cout << setw(32) << f_Dot[0] <<  setw(32) << f_Dot[1] << endl << endl;
  }*/
  return true;
}

bool AbstractWiimote::CalcWiimoteRelativeCursorPos(input_payload_wiimote &f_WiimoteMsg, double f_dSensBarLedDist)
{
  Vect3D_t l_Dot[2], l_RelPos,
           l_CameraRes(g_dWiimoteXCamResolution, g_dWiimoteYCamResolution, 1.0);

  if (!FindSensorBarDots(f_WiimoteMsg.irdot, f_WiimoteMsg.nrdots, l_Dot))
    return false;

  //smooth the ir dots
  l_Dot[0] = m_pSensBarDotSmoother[0]->Smooth(l_Dot[0]);
  l_Dot[1] = m_pSensBarDotSmoother[1]->Smooth(l_Dot[1]);

  //store the raw smoothed sensor bar dots in the message payload (used for head tracking)
  f_WiimoteMsg.SensorBarDot[0].sx = l_Dot[0].x; f_WiimoteMsg.SensorBarDot[0].sy = l_Dot[0].y;
  f_WiimoteMsg.SensorBarDot[1].sx = l_Dot[1].x; f_WiimoteMsg.SensorBarDot[1].sy = l_Dot[1].y;

  //invert the x and y axis to correspond to screen coordinates
  l_Dot[0] = l_CameraRes - l_Dot[0];
  l_Dot[1] = l_CameraRes - l_Dot[1];

  //calc the angle of the wiimote with respect to the sensorbar
  Vect3D_t l_delta = l_Dot[1] - l_Dot[0];
  double theta;
  if (l_delta.x != 0.0)
  {
    theta = -atan(l_delta.y/l_delta.x);
  }
  else
  {
    theta = l_delta.y > 0.0? -M_PI/2.0: M_PI/2.0; 
  }

  //compute the xy coordinates relative to the center of the wiimote camera
  Vect3D_t l_CameraCenter = l_CameraRes / 2.0,
           l_RelFromCenter = ((l_Dot[0] + l_Dot[1])/2.0) - l_CameraCenter;
  
  //rotate the position around the camera center position to compensate for wiimote rotation
  f_WiimoteMsg.relX = l_CameraCenter.x + cos(theta)*l_RelFromCenter.x - sin(theta)*l_RelFromCenter.y;
  f_WiimoteMsg.relY = l_CameraCenter.y + sin(theta)*l_RelFromCenter.x + cos(theta)*l_RelFromCenter.y;
  
  //now devide the rotated pixel coordinates by the camera resolution to get values in the range [0,1]?? <- not true
  f_WiimoteMsg.relX /= l_CameraRes.x;
  f_WiimoteMsg.relY /= l_CameraRes.y;

  f_WiimoteMsg.Zdist = CalcZDistInMM(l_Dot, f_dSensBarLedDist);

  //cout << "Z_mm: " << std::fixed << std::setprecision(2) << setw(10) << left << f_pRelPos->z ;
  return true;
}

#ifdef USE_WIIYOURSELF

#include <wiimote.h>

WiiYourselfWiimote::WiiYourselfWiimote()
{
  m_pWm = new wiimote();
  m_pWm->ChangedCallback = NULL; //no callbacks, we just poll...
	m_pWm->CallbackTriggerFlags = NO_CHANGE;
}

WiiYourselfWiimote::~WiiYourselfWiimote()
{
  if (m_pWm->IsConnected())
  {
    m_pWm->Disconnect();
  }

  delete m_pWm;
}

bool WiiYourselfWiimote::Connect()
{
  if (m_pWm->Connect())
  {
    //connected set ir report mode
    m_pWm->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true);
    return true;
  }
  else
  {
    return false;
  }
}

void WiiYourselfWiimote::StartRumble()
{
  m_pWm->SetRumble(true);
}

void WiiYourselfWiimote::StopRumble()
{
  m_pWm->SetRumble(false);
}

void WiiYourselfWiimote::SetLeds(unsigned char bitmask)
{
  m_pWm->SetLEDs(bitmask);
}

bool WiiYourselfWiimote::IsConnected()
{
  return m_pWm->IsConnected();
}

void WiiYourselfWiimote::ParseWiimote(input_payload_wiimote &f_payload)
{
  m_pWm->RefreshState();

  f_payload.nrdots = 0;

  for (int i=0; i<4; i++)
  {
    if (m_pWm->IR.Dot[i].bFound)
    {
      f_payload.irdot[f_payload.nrdots].rx = m_pWm->IR.Dot[i].RawX;
      f_payload.irdot[f_payload.nrdots].ry = m_pWm->IR.Dot[i].RawY;
      f_payload.nrdots++;
    }
  }

  f_payload.btns = 0;
  f_payload.btns |= (m_pWm->Button.A()?     WIIMOTE_BUTTON_A      : 0) |
                    (m_pWm->Button.B()?     WIIMOTE_BUTTON_B      : 0) |
                    (m_pWm->Button.Up() ?   WIIMOTE_BUTTON_UP     : 0) |
                    (m_pWm->Button.Down()?  WIIMOTE_BUTTON_DOWN   : 0) |
                    (m_pWm->Button.Left()?  WIIMOTE_BUTTON_LEFT   : 0) |
                    (m_pWm->Button.Right()? WIIMOTE_BUTTON_RIGHT  : 0) |
                    (m_pWm->Button.One()?   WIIMOTE_BUTTON_ONE    : 0) |
                    (m_pWm->Button.Two()?   WIIMOTE_BUTTON_TWO    : 0) |
                    (m_pWm->Button.Plus()?  WIIMOTE_BUTTON_PLUS   : 0) |
                    (m_pWm->Button.Minus()? WIIMOTE_BUTTON_MINUS  : 0) |
                    (m_pWm->Button.Home()?  WIIMOTE_BUTTON_HOME   : 0);
}

#endif //USE_WIIYOURSELF