#include "pch.h"

#include <iostream>
#include <thread>



#include "Conversion.hpp"

#include "MT_linear.hpp"
#include "MT_channel.hpp"




/**************************************/
/******** LINEAR_TO_NON_LINEAR ********/
/**************************************/

float Conversion::linear_to_non_linear(float data)
{
  if (data <= 0.0031308f)
    return data * 12.92f;

  return 1.055f * powf(data, 0.4166666667f) - 0.055f;
}

#ifdef _MT_

void* linear_to_non_linear_MT(void* arg)
{
  MT_linear* a = (MT_linear*)arg;

  const float* data = a->data;

  float* result = a->result;

  for (unsigned int i = 0; i < a->length; i++)
    result[i] = Conversion::linear_to_non_linear(data[i]);

  return arg;
}

float* Conversion::linear_to_non_linear(const float* data, unsigned int length)
{
  float* non_linear = new float[length];
  std::thread tab_t[_MT_];
  MT_linear tab_a[_MT_];

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size);
    tab_a[id].length = block_size;
    tab_a[id].result = non_linear + (id * block_size);

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

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

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

  return non_linear;
}

#else

float* Conversion::linear_to_non_linear(const float* data, unsigned int length)
{
  float* non_linear = new float[length];
  for (unsigned int i = 0; i < length; i++)
    non_linear[i] = linear_to_non_linear(data[i]);

  return non_linear;
}

#endif



/**************************************/
/******** NON_LINEAR_TO_LINEAR ********/
/**************************************/

float Conversion::non_linear_to_linear(float data)
{
  if (data <= 0.040449936f)
    return data / 12.92f;

  return powf((data + 0.055f) / 1.055f, 2.4f);
}


#ifdef _MT_

void* non_linear_to_linear_MT(void* arg)
{
  MT_linear* a = (MT_linear*)arg;

  const float* data = a->data;
  float* result = a->result;

  for (unsigned int i = 0; i < a->length; i++)
    result[i] = Conversion::non_linear_to_linear(data[i]);

  return arg;
}

float* Conversion::non_linear_to_linear(const float* data, unsigned int length)
{
  float* linear = new float[length];
  std::thread tab_t[_MT_];
  MT_linear tab_a[_MT_];

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size);
    tab_a[id].length = block_size;
    tab_a[id].result = linear + (id * block_size);

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

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

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

  return linear;
}

#else

float* Conversion::non_linear_to_linear(const float* data, unsigned int length)
{
  float* linear = new float[length];
  for (unsigned int i = 0; i < length; i++)
    linear[i] = non_linear_to_linear(data[i]);

  return linear;
}

#endif



/*************************************/
/************ sRGB_TO_XYZ ************/
/*************************************/

std::tuple<float, float, float> Conversion::sRGB_to_XYZ(float r, float g, float b)
{
  float x = r * Conversion::sRGB_to_XYZ_m[0][0] + g * Conversion::sRGB_to_XYZ_m[0][1] + b * Conversion::sRGB_to_XYZ_m[0][2];
  float y = r * Conversion::sRGB_to_XYZ_m[1][0] + g * Conversion::sRGB_to_XYZ_m[1][1] + b * Conversion::sRGB_to_XYZ_m[1][2];
  float z = r * Conversion::sRGB_to_XYZ_m[2][0] + g * Conversion::sRGB_to_XYZ_m[2][1] + b * Conversion::sRGB_to_XYZ_m[2][2];
  return std::make_tuple(x, y, z);
}

#ifdef _MT_

void* sRGB_to_XYZ_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::sRGB_to_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::sRGB_to_XYZ(const float* data, const unsigned int length)
{
  float* xyz = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = xyz + (id * block_size * 3);

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

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

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

  return xyz;
}

#else

float* Conversion::sRGB_to_XYZ(const float* data, const unsigned int length)
{
  float* xyz = new float[length * 3];
  for (unsigned int i = 0; i < length; i++)
  {
    std::tuple<float, float, float> conv = sRGB_to_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    xyz[i * 3] = std::get<0>(conv);
    xyz[i * 3 + 1] = std::get<1>(conv);
    xyz[i * 3 + 2] = std::get<2>(conv);
  }
  return xyz;
}

#endif

float Conversion::sRGB_to_Y_of_XYZ(float r, float g, float b)
{
  return (r * Conversion::sRGB_to_XYZ_m[1][0] + g * Conversion::sRGB_to_XYZ_m[1][1] + b * Conversion::sRGB_to_XYZ_m[1][2]);
}

#ifdef _MT_

void* sRGB_to_Y_of_XYZ_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* channel = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
    channel[i] = Conversion::sRGB_to_Y_of_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);

  return arg;
}

float* Conversion::sRGB_to_Y_of_XYZ(const float* data, const unsigned int length)
{
  float* channelY = new float[length];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelY + (id * block_size);

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

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

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

  return channelY;
}

#else

float* Conversion::sRGB_to_Y_of_XYZ(const float* data, const unsigned int length)
{
  float* y = new float[length];
  for (unsigned int i = 0; i < length; i++)
    y[i] = sRGB_to_Y_of_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);

  return y;
}

#endif



/*************************************/
/************* XYZ_TO_LAB ************/
/*************************************/

std::tuple<float, float, float> Conversion::XYZ_to_Lab(float x, float y, float z)
{
  float xNorm = x / 0.950455927f;
  float yNorm = y;
  float zNorm = z / 1.08905775f;

  float coeff = 16.0f / 116.0f;

  float fx = 7.787f * xNorm + coeff;
  float fy = 7.787f * yNorm + coeff;
  float fz = 7.787f * zNorm + coeff;

  if (xNorm > 0.008856f)
    fx = powf(xNorm, 0.3333333333f);
  if (yNorm > 0.008856f)
    fy = powf(yNorm, 0.3333333333f);
  if (zNorm > 0.008856f)
    fz = powf(zNorm, 0.3333333333f);

  return std::make_tuple(116.0f * fy - 16.0f, 500.0f * (fx - fy), 200.0f * (fy - fz));
}

#ifdef _MT_

void* XYZ_to_Lab_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::XYZ_to_Lab(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::XYZ_to_Lab(const float* data, const unsigned int length)
{
  float* channelLab = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelLab + (id * block_size * 3);

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

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

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

  return channelLab;
}

#else

float* Conversion::XYZ_to_Lab(const float* data, const unsigned int length)
{
  float* channelLab = new float[length*3];
  for (unsigned int i = 0; i < length; i++)
    {
      std::tuple<float, float, float> lab = XYZ_to_Lab(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
      channelLab[i*3] = std::get<0>(lab);
      channelLab[i*3+1] = std::get<1>(lab);
      channelLab[i*3+2] = std::get<2>(lab);
    }

  return channelLab;
}

#endif



/*************************************/
/************ SRGB_TO_LAB ************/
/*************************************/

std::tuple<float, float, float> Conversion::sRGB_to_Lab(float r, float g, float b)
{
  std::tuple<float, float, float> xyz = sRGB_to_XYZ(r, g, b);
  return XYZ_to_Lab(std::get<0>(xyz),std::get<1>(xyz),std::get<2>(xyz));
}


float* Conversion::sRGB_to_Lab(const float* data, const unsigned int length)
{
  float* rgb_to_xyz = sRGB_to_XYZ(data, length);
  float* lab = XYZ_to_Lab(rgb_to_xyz,length);
  delete[](rgb_to_xyz);
  return lab;
}

float Conversion::sRGB_to_L_of_Lab(float r, float g, float b)
{
  std::tuple<float, float, float> xyz = sRGB_to_XYZ(r, g, b);

  float fy = 7.787f * std::get<1>(xyz) + (16.0f / 116.0f);

  if (std::get<1>(xyz) > 0.008856f)
    fy = powf(std::get<1>(xyz), 1.0f / 3.0f);


  return (116.0f * fy - 16.0f);
}



/************************************/
/************ LAB_TO_LCH ************/
/************************************/

std::tuple<float, float, float> Conversion::Lab_to_LCH(float L, float a, float b)
{
  float C = sqrtf(a * a + b * b);
  float theta = atan2(b, a);
  while (theta < 0)
    theta += (float)(2.0f * M_PI);
  while (theta > (float)(2.0f * M_PI))
    theta -= (float)(2.0f * M_PI);
  float H = theta / ((float)M_PI) * 180.0f;
  return std::make_tuple(L, C, H);
}

#ifdef _MT_

void* Lab_to_LCH_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::Lab_to_LCH(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::Lab_to_LCH(const float* data, const unsigned int length)
{
  float* channelLCH = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelLCH + (id * block_size * 3);

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

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

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

  return channelLCH;
}

#else

float* Conversion::Lab_to_LCH(const float* data, const unsigned int length)
{
  float* LCH = new float[length * 3];
  for (unsigned int i = 0; i < length; i++)
  {
    std::tuple<float, float, float> conv = Lab_to_LCH(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    LCH[i * 3] = std::get<0>(conv);
    LCH[i * 3 + 1] = std::get<1>(conv);
    LCH[i * 3 + 2] = std::get<2>(conv);
  }
  return LCH;
}

#endif

float Conversion::Lab_to_C_of_LCH(float a, float b)
{
  return sqrtf(a * a + b * b);
}


float Conversion::Lab_to_H_of_LCH(float a, float b)
{
  float theta = atan2(b, a);
  while (theta < 0)
    theta += (float)(2.0f * M_PI);
  while (theta > (float)(2.0f * M_PI))
    theta -= (float)(2.0f * M_PI);
  return theta / ((float)M_PI) * 180.0f;
}



/************************************/
/************ LCH_TO_LAB ************/
/************************************/

std::tuple<float, float, float> Conversion::LCH_to_Lab(float L, float C, float H)
{
  float rho = C;
  float phi = (H/180.0f)*((float)M_PI);
  float a = rho*cos(phi);
  float b = rho*sin(phi);
  return std::make_tuple(L,a,b);
}

#ifdef _MT_

void* LCH_to_Lab_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::LCH_to_Lab(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::LCH_to_Lab(const float* data, const unsigned int length)
{
  float* channelLab = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelLab + (id * block_size * 3);

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

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

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

  return channelLab;
}

#else

float* Conversion::LCH_to_Lab(const float* data, const unsigned int length)
{
  float* Lab = new float[length * 3];
  for (unsigned int i = 0; i < length; i++)
  {
    std::tuple<float, float, float> conv = LCH_to_Lab(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    Lab[i * 3] = std::get<0>(conv);
    Lab[i * 3 + 1] = std::get<1>(conv);
    Lab[i * 3 + 2] = std::get<2>(conv);
  }
  return Lab;
}

#endif



/************************************/
/************ LAB_TO_XYZ ************/
/************************************/

std::tuple<float, float, float> Conversion::Lab_to_XYZ(float L, float a, float b)
{
  float fy = (L+16.0f)/116.0f;
  float fx = a/500.0f+fy;
  float fz = fy - b/200.0f;

  float xNorm = 0.950455927f*(fx - 0.137931034f)*0.128418549f;
  float yNorm = (fy - 0.137931034f)*0.128418549f;
  float zNorm = 1.08905775f*(fz - 0.137931034f)*0.128418549f;

  if(fx>0.206896552f)
    xNorm = 0.950455927f*fx*fx*fx;
  if(fy>0.206896552f)
    yNorm = fy*fy*fy;
  if(fz>0.206896552f)
    zNorm = 1.08905775f*fz*fz*fz;
  
  return std::make_tuple(xNorm,yNorm,zNorm);
}

#ifdef _MT_

void* Lab_to_XYZ_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::Lab_to_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::Lab_to_XYZ(const float* data, const unsigned int length)
{
  float* channelXYZ = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelXYZ + (id * block_size * 3);

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

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

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

  return channelXYZ;
}

#else

float* Conversion::Lab_to_XYZ(const float* data, const unsigned int length)
{
  float* Lab = new float[length * 3];
  for (unsigned int i = 0; i < length; i++)
  {
    std::tuple<float, float, float> conv = Lab_to_XYZ(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    Lab[i * 3] = std::get<0>(conv);
    Lab[i * 3 + 1] = std::get<1>(conv);
    Lab[i * 3 + 2] = std::get<2>(conv);
  }
  return Lab;
}

#endif



/*************************************/
/************ XYZ_TO_sRGB ************/
/*************************************/

std::tuple<float, float, float> Conversion::XYZ_to_sRGB(float x, float y, float z)
{
  float r = x * Conversion::XYZ_to_sRGB_m[0][0] + y * Conversion::XYZ_to_sRGB_m[0][1] + z * Conversion::XYZ_to_sRGB_m[0][2];
  float g = x * Conversion::XYZ_to_sRGB_m[1][0] + y * Conversion::XYZ_to_sRGB_m[1][1] + z * Conversion::XYZ_to_sRGB_m[1][2];
  float b = x * Conversion::XYZ_to_sRGB_m[2][0] + y * Conversion::XYZ_to_sRGB_m[2][1] + z * Conversion::XYZ_to_sRGB_m[2][2];
  return std::make_tuple(r, g, b);
}

#ifdef _MT_

void* XYZ_to_sRGB_MT(void* arg)
{
  MT_channel* a = (MT_channel*)arg;

  const float* data = a->data;
  float* result = a->channel;

  for (unsigned int i = 0; i < a->length; i++)
  {
    std::tuple<float, float, float> v = Conversion::XYZ_to_sRGB(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    result[i * 3] = std::get<0>(v);
    result[i * 3 + 1] = std::get<1>(v);
    result[i * 3 + 2] = std::get<2>(v);
  }

  return arg;
}

float* Conversion::XYZ_to_sRGB(const float* data, const unsigned int length)
{
  float* channelRGB = new float[length * 3];

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

  unsigned int id;
  unsigned int block_size = length / _MT_;
  for (id = 0; id < _MT_; id++) {
    tab_a[id].data = data + (id * block_size * 3);
    tab_a[id].length = block_size;
    tab_a[id].channel = channelRGB + (id * block_size * 3);

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

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

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

  return channelRGB;
}

#else

float* Conversion::XYZ_to_sRGB(const float* data, const unsigned int length)
{
  float* rgb = new float[length * 3];
  for (unsigned int i = 0; i < length; i++)
  {
    std::tuple<float, float, float> conv = XYZ_to_sRGB(data[i * 3], data[i * 3 + 1], data[i * 3 + 2]);
    rgb[i * 3] = std::get<0>(conv);
    rgb[i * 3 + 1] = std::get<1>(conv);
    rgb[i * 3 + 2] = std::get<2>(conv);
  }
  return rgb;
}

#endif



/*************************************/
/************ LCH_TO_sRGB ************/
/*************************************/

float* Conversion::LCH_to_sRGB(const float* data, const unsigned int length)
{
  float* Lab = LCH_to_Lab(data,length);
  float* xyz = Lab_to_XYZ(Lab,length);
  float* rgb=  XYZ_to_sRGB(xyz,length);
  delete[](Lab);
  delete[](xyz);
  return rgb;
}