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

#include "C_3DPointSmoother.h"

using namespace std;

void C_3DPointSmoother::SetSimpleMovingAverage(int f_iWindowSize)
{
  //reset smoothing stuff for Simple moving average smoothing
  m_Method = SimpMovingAvg;
  m_iWindowSize = f_iWindowSize;
  m_iSampCount = 0;
  m_St = Vect3D_t(0.0, 0.0, 0.0);
  m_SampWindow.clear();
}

void C_3DPointSmoother::SetExponentialMovingAverage(double f_dExponent)
{
  m_Method = ExpMovingAvg;
  m_dExponent = f_dExponent;
  m_iSampCount = 0;
  m_St = Vect3D_t(0.0, 0.0, 0.0);
}

Vect3D_t C_3DPointSmoother::SmoothSample_SimpMovingAvg(const Vect3D_t &f_Xt)
{
  //add sample to the window
  m_SampWindow.push_front(f_Xt);

  if (m_iSampCount < m_iWindowSize)
  {
    //we don't have enough cmopute the average of the points currently in the window
    Vect3D_t l_sum;
    for (std::list<Vect3D_t>::iterator it = m_SampWindow.begin(); it != m_SampWindow.end(); it++)
      l_sum += *it;
    return l_sum / (double)m_SampWindow.size();
  }
  else
  {
    //there are mote than m_iWindowSize samples, incrementally update smoothed sample
    Vect3D_t l_Xtk = m_SampWindow.back(); //pop the last point from the (window) list
    m_SampWindow.pop_back(); 
    return m_St + ((f_Xt - l_Xtk)/(double)m_iWindowSize);
  }
}

Vect3D_t C_3DPointSmoother::SmoothSample_ExpMovingAvg(const Vect3D_t &f_Xt)
{
  if (m_iSampCount == 0)
    return f_Xt;
  else
    return m_St + m_dExponent * (f_Xt - m_St);
}

Vect3D_t C_3DPointSmoother::Smooth(const Vect3D_t &f_Xt)
{
  switch(m_Method)
  {
  case SimpMovingAvg:
      m_St = SmoothSample_SimpMovingAvg(f_Xt);
    break;

  case ExpMovingAvg:
      m_St = SmoothSample_ExpMovingAvg(f_Xt);
    break;
  }

  m_iSampCount++;
  return m_St;
}



#include "wiimote_state.h"

bool FindSensorBarDots(wiimote_state::ir &f_IR, Vect3D_t f_Dots[2])
{
  RawDot l_rd[4];

  int n = 0;
  for(int i = 0; i < 4; i++)
  {
    if (f_IR.Dot[i].bFound)
    {
      l_rd[n].rx = f_IR.Dot[i].RawX;
      l_rd[n].ry = f_IR.Dot[i].RawY;
      n++;

    }
  }

  ////print raw dots
  //cout << "Raw IR dots:" << endl;
  //for (int i=0; i<4; i++)
  //{
  //  if (i < n)
  //  {
  //    cout << "\t" << l_rd[i] << SPACES << endl;
  //  }
  //  else
  //  {
  //    cout << "\t--  --  --" << SPACES << endl;
  //  }
  //}

  return FindSensorBarDots(l_rd, n, f_Dots);
}


bool FindSensorBarDots(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
  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;
}