#include "treatment.hh"

bool Treatment::apExists(const string &ap_addr)const
  {

    string str;

    for (unsigned int i = 0 ; i < access_point_list.size() ; i++)
      {
        str = access_point_list[i].getApAddr() ;
        const int length = str.length() ;
        for (int j = 0 ; j < length ; ++j)
          str[j] = tolower(str[j]) ;
        if (str == ap_addr)
          {
            return true ;
          }
      }

    return false ;
  }

unsigned int Treatment::apIndex(const string &ap_addr)const
  {
    unsigned int i;
    string str;

    for (i = 0 ; i < access_point_list.size() ; i++)
      {
        str = access_point_list[i].getApAddr() ;
        const int length = str.length() ;
        for (int j = 0 ; j < length ; ++j)
          str[j] = tolower(str[j]) ;
        if (str == ap_addr)
          {
            return i;
          }
      }

    return 0; // Should never happen
  }

void Treatment::makeMeasurementList(vector<couple_info> recv_info)
{

  bool inserted = false;
  Measurement measure;
  char *ret = (char *)malloc(sizeof(char) * 18) ;
  string mac_a;
  int value;

  for (unsigned int i = 0; i < recv_info.size(); i++)
    {

      sprintf(ret, "%02x:%02x:%02x:%02x:%02x:%02x", recv_info.at(i).ap_mac_addr_bytes[0], recv_info.at(i).ap_mac_addr_bytes[1], recv_info.at(i).ap_mac_addr_bytes[2], recv_info.at(i).ap_mac_addr_bytes[3], recv_info.at(i).ap_mac_addr_bytes[4], recv_info.at(i).ap_mac_addr_bytes[5]) ;
      ret[17] = '\0';
      mac_a = ret;
      value = recv_info.at(i).antenna_signal_dbm;

      for (unsigned int j = 0 ; j < m.size() ; j++)
        if (m[j].getMacAddr() == mac_a)
          {
            m[j].addSsValue(value);
            inserted = true;
            break;
          }
      if (inserted == false)
        {
          measure.setMacAddr(mac_a);
          measure.addSsValue(value);
          m.push_back(measure);
        }
    }

  free(ret);
}

vector<Point> Treatment::getkClosestInSs(const unsigned int &k, const Point *point_ignored)const
  {

    unsigned int i, j, min_idx;
    vector<float> distances_vector;
    vector<Point> points_vector;
    Point tmp_pt;
    float tmp_distance = 0, dist_max = 10000000, tmp_min;

    for (i = 0 ; i < reference_point_list.size() ; i++)
      if (point_ignored == NULL || (reference_point_list[i].getCoordinates() != *point_ignored))
        {
          tmp_distance = reference_point_list[i].getSsSquareDistance(m);

          /* if not k points, add it */
          if (distances_vector.size() < k)
            {
              distances_vector.push_back(tmp_distance);
              points_vector.push_back(reference_point_list[i].getCoordinates());
              dist_max = (dist_max < tmp_distance) ? tmp_distance : dist_max;
            }
          else
            {
              /* if tmp_dst < dist_max, should add it and remove previous greatest dist. */
              if (dist_max > tmp_distance)
                {
                  /* remove old max */
                  for (j = 0 ; j < distances_vector.size() ; j++)
                    if (distances_vector[j] == dist_max)
                      {
                        distances_vector[j] = tmp_distance;
                        points_vector[j] = reference_point_list[i].getCoordinates();
                        break;
                      }
                  /* Now seek the new max. distance */
                  dist_max = distances_vector[0];
                  for (j = 1 ; j < distances_vector.size() ; j++)
                    if (distances_vector[j] > dist_max)
                      dist_max = distances_vector[j];
                }
              /* Else nothing needs to be done */
            }
        }

    /* Sorts the vector */
    for (i = 0 ; i < distances_vector.size() - 1 ; i++)
      {
        tmp_min = distances_vector[i];
        min_idx = i;
        for (j = i+1 ; j < distances_vector.size() ; j++)
          if (tmp_min > distances_vector[j])
            {
              tmp_min = distances_vector[j];
              min_idx = j;
            }

        if (min_idx != i)
          {
            /* Swap points */
            tmp_pt = points_vector[i];
            points_vector[i] = points_vector[min_idx];
            points_vector[min_idx] = tmp_pt;

            /* Swap distances */
            distances_vector[min_idx] = distances_vector[i];
            distances_vector[i] = tmp_min;
          }
      }

    return points_vector;
  }

Point Treatment::fbcm(const int &client_idx)const
  {

    Point ret(0, 0, 0);
    vector<string> addr;
    vector<float> dist_vect;
    vector<Point> centres;
    unsigned int i, ap_idx;
    float constant_term, minmax_res, minmax_max;
    float x = MINMAX_X_START, y = MINMAX_Y_START, z = MINMAX_Z_START;

    i = 0;
    //cout << "FBCM: ";
    for (i = 0 ; i < m.size() ; i++)
      if (apExists(m[i].getMacAddr()))
        {
          ap_idx = apIndex(m[i].getMacAddr());
          //cout << "AP idx: " << ap_idx << " ";
          centres.push_back(access_point_list[ap_idx].getCoordinates());
          addr.push_back(m[i].getMacAddr());
          constant_term = access_point_list[ap_idx].getOutputPower() + access_point_list[ap_idx].getAntennaGain() + 2;
          constant_term += 20 * log10((300000000.0 / (float) access_point_list[ap_idx].getFrequency()) / (4*M_PI));
          //end of expr. should be: client_list[client_idx].getAntennaGain() instead of 2.
          //cout << "20log(" << (300000000.0 / (float) access_point_list[ap_idx].getFrequency()) / (4*M_PI) << ") = ";
          //cout << constant_term << " ";
          dist_vect.push_back(pow(10, (constant_term - m[i].getAverage()) / (10 * access_point_list[ap_idx].getFriisIndex())));
          //cout << endl;
        }

    /* Then: min-max */
    minmax_res = 1000000;
    for (x = MINMAX_X_START ; x < MINMAX_X_STOP ; x += MINMAX_STEP)
      for (y = MINMAX_Y_START ; y < MINMAX_Y_STOP ; y += MINMAX_STEP)
        for (z = MINMAX_Z_START ; z <= MINMAX_Z_STOP ; z += MINMAX_STEP)
          {
            minmax_max = 0;
            for (i = 0 ; i < centres.size() ; i++)
              if (abs(centres[i].distance(x, y, z) - dist_vect[i]) > minmax_max)
                minmax_max = abs(centres[i].distance(x, y, z) - dist_vect[i]) ;
            if (minmax_max < minmax_res)
              {
                ret.setX(x);
                ret.setY(y);
                ret.setZ(z);
                minmax_res = minmax_max;
              }
          }

    /* Clear all vectors */
    addr.clear();
    dist_vect.clear();
    centres.clear();

    /* Return position */
    return ret;
  }

Point Treatment::fbcm_friis(const vector<float> friis_idx_list, const float &z)const
  {

    Point ret(0, 0, 0);
    vector<float> dist_vect;
    vector<Point> centres;
    unsigned int i, ap_idx;
    float constant_term, minmax_res, minmax_max;
    float x = MINMAX_X_START, y = MINMAX_Y_START ;
    vector<Measurement> vm = m; //Used when filtering 3 strongest APs
    vector<float> friis_idx = friis_idx_list; //Used when filtering 3 strongest APs

    i = 0;
    for (i = 0 ; i < vm.size() ; i++)
      {
        if (apExists(vm[i].getMacAddr()))
          {
            ap_idx = apIndex(vm[i].getMacAddr());
            constant_term = access_point_list[ap_idx].getOutputPower() + access_point_list[ap_idx].getAntennaGain() + 2;
            constant_term += 20 * log10((300000000.0 / (float) access_point_list[ap_idx].getFrequency()) / (4*M_PI));
            if (friis_idx[i] != -1)
              {
                centres.push_back(access_point_list[ap_idx].getCoordinates());
                dist_vect.push_back(pow(10, (constant_term - vm[i].getAverage()) / (10 * friis_idx[i])));
              }
          }
      }


    /* Then: min-max */
    minmax_res = 1000000;
    for (x = MINMAX_X_START ; x < MINMAX_X_STOP ; x += MINMAX_STEP)
      for (y = MINMAX_Y_START ; y < MINMAX_Y_STOP ; y += MINMAX_STEP)
        {
          minmax_max = 0;
          for (i = 0 ; i < centres.size() ; i++)
            if (abs(centres[i].distance(x, y, z) - dist_vect[i]) > minmax_max)
              minmax_max = abs(centres[i].distance(x, y, z) - dist_vect[i]) ;
          if (minmax_max < minmax_res)
            {
              ret.setX(x);
              ret.setY(y);
              ret.setZ(z);
              minmax_res = minmax_max;
            }
        }

    /* Clear all vectors */
    dist_vect.clear();
    centres.clear();

    /* Return position */
    return ret;
  }

Point Treatment::interlink(const int &client_idx) const
  {

    Point ret(0, 0, 0);
    vector<string> addr;
    vector<float> dist_vect;
    vector<Point> centres;
    unsigned int i, ap_idx;
    float constant_term, minmax_res, minmax_max;
    float x = MINMAX_X_START, y = MINMAX_Y_START, z = MINMAX_Z_START;

    i = 0;
    for (i = 0 ; i < m.size() ; i++)
      if (apExists(m[i].getMacAddr()))
        {
          ap_idx = apIndex(m[i].getMacAddr());
          centres.push_back(access_point_list[ap_idx].getCoordinates());
          addr.push_back(m[i].getMacAddr());
          constant_term = access_point_list[ap_idx].getOutputPower() + access_point_list[ap_idx].getAntennaGain();
          constant_term += 20 * log10((300000000.0 / (float) access_point_list[ap_idx].getFrequency()) / (4*M_PI)) + 2;
          //end of expr. should be: client_list[client_idx].getAntennaGain() instead of 2.
          dist_vect.push_back(pow(10, (constant_term - m[i].getAverage()) / 35));
        }

    /* Then: min-max */
    minmax_res = 1000000;
    for (x = MINMAX_X_START ; x < MINMAX_X_STOP ; x += MINMAX_STEP)
      for (y = MINMAX_Y_START ; y < MINMAX_Y_STOP ; y += MINMAX_STEP)
        for (z = MINMAX_Z_START ; z < MINMAX_Z_STOP ; z += MINMAX_STEP)
          {
            minmax_max = 0;
            for (i = 0 ; i < centres.size() ; i++)
              if (abs(centres[i].distance(x, y, z) - dist_vect[i]) > minmax_max)
                minmax_max = abs(centres[i].distance(x, y, z) - dist_vect[i]);
            if (minmax_max < minmax_res)
              {
                ret.setX(x);
                ret.setY(y);
                ret.setZ(z);
                minmax_res = minmax_max;
              }
          }

    /* Clear all vectors */
    addr.clear();
    dist_vect.clear();
    centres.clear();

    /* Return position */
    return ret;
  }

vector<float> Treatment::computeFriisFromRefList(const Point &p)
{

  vector<float> friis_idx_list;
  Point pt_coords, ap_coords;
  float ap_power, ap_gain, calib_gain = 2, const_term, mes_power/*, friis_sum*/;
  unsigned int ap_freq;
  string ap_mac;


  /* Compute an index for each ref point. List stored in friis_idx_list */
  /* Compute an index for Point p. List stored in friis_idx_list */
  for (unsigned int j = 0 ; j < reference_point_list.size() ; j++)
    {
      pt_coords = reference_point_list[j].getCoordinates();
      if (pt_coords==p)
        {
          for (unsigned int i = 0 ; i < m.size() ; i++)
            {
              if (apExists(m[i].getMacAddr()))
                {
                  unsigned int ap_idx = apIndex(m[i].getMacAddr());
                  ap_power = access_point_list[ap_idx].getOutputPower();
                  ap_coords = access_point_list[ap_idx].getCoordinates();
                  ap_freq = access_point_list[ap_idx].getFrequency();
                  ap_gain = access_point_list[ap_idx].getAntennaGain();
                  ap_mac = access_point_list[ap_idx].getApAddr();

                  /* Compute main general term, independant from scans */
                  const_term = calib_gain + ap_gain;
                  const_term -= 20 * log10(4 * M_PI);
                  const_term += 20 * log10 (300000000.0 / ap_freq) + ap_power;

                  if (reference_point_list[j].getPowerForAp(ap_mac, &mes_power))
                    friis_idx_list.push_back((const_term - mes_power) / (10 * log10(ap_coords.distance(pt_coords))));
                  else
                    friis_idx_list.push_back(-1);
                }
            }
          break;
        }
    }

  return friis_idx_list;
}

Point Treatment::getkWeightedInSs(const unsigned int &k, const Point *point_ignored)const
  {

    unsigned int i, j;
    vector<float> distances_vector;
    vector<Point> points_vector;
    float tmp_distance = 0, dist_max = 10000000;
    Point ret;
    float total = 0, x = 0, y = 0, z = 0;

    for (i = 0 ; i < reference_point_list.size() ; i++)
      if (point_ignored == NULL || (reference_point_list[i].getCoordinates() != *point_ignored))
        {
          tmp_distance = reference_point_list[i].getSsSquareDistance(m);
          /* if not k points, add it */
          if (distances_vector.size() < k)
            {
              distances_vector.push_back(tmp_distance);
              points_vector.push_back(reference_point_list[i].getCoordinates());
              dist_max = (dist_max < tmp_distance) ? tmp_distance : dist_max;
            }
          else
            {
              /* if tmp_dst < dist_max, should add it and remove previous greatest dist. */
              if (dist_max > tmp_distance)
                {
                  /* remove old max */
                  for (j = 0 ; j < distances_vector.size() ; j++)
                    if (distances_vector[j] == dist_max)
                      {
                        dist_max = tmp_distance;
                        distances_vector.erase(distances_vector.begin() + j);
                        points_vector.erase(points_vector.begin() + j);
                        distances_vector.push_back(tmp_distance);
                        points_vector.push_back(reference_point_list[i].getCoordinates());
                        break;
                      }
                }
              /* Else nothing needs to be done */
            }
        }
    for (i = 0 ; i < distances_vector.size() ; i++)
      total += (1 / distances_vector[i]);

    for (i = 0 ; i < distances_vector.size() ; i++)
      {
        x += points_vector[i].getX() * (1 / distances_vector[i]) / total;
        y += points_vector[i].getY() * (1 / distances_vector[i]) / total;
        z += points_vector[i].getZ() * (1 / distances_vector[i]) / total;
      }

    ret.setX(x);
    ret.setY(y);
    ret.setZ(z);

    return ret;
  }