#include "pch.h"

#include <iostream>
#include <thread>

#include "Utils.hpp"
#include "MT_interpolation.hpp"

#ifdef _MT_

void* interp_MT(void* arg)
{
  MT_interpolation* a = (MT_interpolation*)arg;

  const float* xData = a->xData;
  float* xInterp = a->xInterp;


  for (unsigned int i = 0; i < a->length; i++)
    {
      unsigned int index = Utils::search(xData[i], a->xd, 0, a->length_xd_yd - 1);
      xInterp[i] = Utils::interp(xData[i], a->xd[index], a->yd[index], a->xd[index + 1], a->yd[index + 1]);
    }

  return arg;
}

float* Utils::interp(const float* x, const unsigned int tx, const Eigen::RowVectorXf& xd, const Eigen::RowVectorXf& yd)
{
  float* xInterp = new float[tx];

  std::thread tab_t[_MT_];
  MT_interpolation tab_a[_MT_];

  unsigned int id;
  unsigned int tab_length = tx;
  unsigned int block_size = tab_length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].xInterp = xInterp + (id * block_size);
    tab_a[id].xData = x + (id * block_size);
    tab_a[id].length = block_size;
    tab_a[id].xd = xd.data();
    tab_a[id].yd = yd.data();
    tab_a[id].length_xd_yd = (unsigned int)(xd.size());

    if (id == (_MT_ - 1))
      tab_a[id].length = tab_length - ((_MT_ - 1) * block_size);

    tab_t[id] = std::thread(interp_MT, (void*)(tab_a + id));
  }

  for (id = 0; id < _MT_; id++) {
    tab_t[id].join();
  }

  return xInterp;
}

#else

float* Utils::interp(const float* x, const unsigned int tx, const Eigen::RowVectorXf& xd, const Eigen::RowVectorXf& yd)
{
  float* xInterp = new float[tx];

  for (unsigned int i = 0; i < tx; i++)
    {
      unsigned int index = Utils::search(x[i], xd);
      xInterp[i] = interp(x[i], xd(index), yd(index), xd(index + 1), yd(index + 1));
    }

  return xInterp;
}

#endif

float Utils::interp(float x, float xd1, float yd1, float xd2, float yd2)
{
  float delta_xd2_xd1 = xd2 - xd1;
  float delta_x_xd1 = x - xd1;
  float delta_yd2_yd1 = yd2 - yd1;
  return yd1 + delta_yd2_yd1 * (delta_x_xd1 / delta_xd2_xd1);
}


// Maybe this method is useless and the index can be determined in the interp metho.
unsigned int Utils::search(float to_search, const Eigen::RowVectorXf& data)
{
  // In case of approximation
  if (to_search <= data[0])
    return 0;

  if (to_search >= data[data.size() - 1])
    return (unsigned int)(data.size() - 2);

  unsigned int first = 0;
  unsigned int last = (unsigned int)(data.size() - 1);

  unsigned int index = (first + last) / 2;

  while (!(to_search >= data[index] && to_search < data[index + 1]))
    {
      if (to_search < data[index])
	last = index;
      else
	first = index;
      index = (first + last) / 2;
    }

  return index;
}

// Maybe this method is useless and the index can be determined in the interp metho.
unsigned int Utils::search(float to_search, const float* data, unsigned int first, unsigned int last)
{
  // In case of approximation
  if (to_search <= data[0])
    return 0;

  if (to_search >= data[last])
    return last - 1;

  unsigned int index = (first + last) / 2;

  while (!(to_search >= data[index] && to_search < data[index + 1]))
    {
      if (to_search < data[index])
	last = index;
      else
	first = index;
      index = (first + last) / 2;
    }

  return index;
}


// Parallelization is unless for this method because it is used in an already parallelized method.
float* Utils::NPlinearWeightMask(float* x, unsigned int length, float xMin, float xMax, float xTolerance)
{
  float* y = new float[length];

  for(unsigned int i=0;i<length;i++)
    y[i]=1;

  for(unsigned int i=0;i<length;i++)
    if(x[i]<=(xMin-xTolerance))
      y[i]=0;

  for(unsigned int i=0;i<length;i++)
    if((x[i]>(xMin-xTolerance)) && (x[i]<=xMin))
      y[i]=(x[i]-(xMin-xTolerance)) / xTolerance;

  for(unsigned int i=0;i<length;i++)
    if((x[i]>xMin) && (x[i]<=xMax))
      y[i]=1;

  for(unsigned int i=0;i<length;i++)
    if((x[i]>xMax) && (x[i]<=(xMax+xTolerance)))
      y[i]=1-((x[i]-xMax) / xTolerance);

  for(unsigned int i=0;i<length;i++)
    if(x[i]>(xMax+xTolerance))
      y[i]=0;
  
  return y;
}


//#endif