hardware-api/include/viewModel/bMode/display/PchipInterp.h

#ifndef PCHIPINTERP_H
#define PCHIPINTERP_H

#include <QObject>
#include <QDebug>

//
//File: pchipInterp.cpp
//
//MATLAB Coder version            : 4.1
//C/C++ source code generated on  : 24-Dec-2020 17:26:59
//

//Include Files
#include <stddef.h>
#include <stdlib.h>
#include <math.h>

//Function Definitions

//Arguments    : float d1
//float d2
//float h1
//float h2
//Return Type  : float
//

float exteriorSlope(float d1, float d2, float h1, float h2)
{
	float s;
	float signd1;
	float signs;
	s = ((2.0F * h1 + h2) * d1 - h1 * d2) / (h1 + h2);
	signd1 = d1;
	if(d1 < 0.0F)
	{
		signd1 = -1.0F;
	}
	else
	if(d1 > 0.0F)
	{
		signd1 = 1.0F;
	}
	else
	{
		if(d1 == 0.0F)
		{
			signd1 = 0.0F;
		}
	}

	signs = s;
	if(s < 0.0F)
	{
		signs = -1.0F;
	}
	else
	if(s > 0.0F)
	{
		signs = 1.0F;
	}
	else
	{
		if(s == 0.0F)
		{
			signs = 0.0F;
		}
	}

	if(signs != signd1)
	{
		s = 0.0F;
	}
	else
	{
		signs = d2;
		if(d2 < 0.0F)
		{
			signs = -1.0F;
		}
		else
		if(d2 > 0.0F)
		{
			signs = 1.0F;
		}
		else
		{
			if(d2 == 0.0F)
			{
				signs = 0.0F;
			}
		}

		if((signd1 != signs) && ((float)fabs((double)s) > (float)fabs((double)(3.0F *
																			   d1))))
		{
			s = 3.0F * d1;
		}
	}

	return s;
}

//Function Definitions

//
//Arguments    : const float x[8]
//const float y[8]
//float p[256]
//Return Type  : void

void pchipInterp(const float x[8], const float y[8], float p[256])
{
	int k;
	float h[7];
	float del[7];
	float slopes[8];
	float dzzdx;
	float hs;
	float hs3;
	float w1;
	int low_i;
	float pp_coefs[28];
	int low_ip1;
	int high_i;
	int mid_i;
	for(k = 0; k < 7; k++)
	{
		h[k] = x[k + 1] - x[k];
	}

	for(k = 0; k < 7; k++)
	{
		del[k] = (y[k + 1] - y[k]) / h[k];
	}

	for(k = 0; k < 6; k++)
	{
		dzzdx = h[1 + k];
		hs = h[k] + dzzdx;
		hs3 = 3.0F * hs;
		w1 = (h[k] + hs) / hs3;
		dzzdx = (dzzdx + hs) / hs3;
		slopes[1 + k] = 0.0F;
		if(del[k] < 0.0F)
		{
			if(del[1 + k] <= del[k])
			{
				slopes[1 + k] = del[k] / (w1 * (del[k] / del[1 + k]) + dzzdx);
			}
			else
			{
				if(del[1 + k] < 0.0F)
				{
					slopes[1 + k] = del[1 + k] / (w1 + dzzdx * (del[1 + k] / del[k]));
				}
			}
		}
		else
		{
			if(del[k] > 0.0F)
			{
				hs = del[1 + k];
				if(hs >= del[k])
				{
					slopes[1 + k] = del[k] / (w1 * (del[k] / hs) + dzzdx);
				}
				else
				{
					if(hs > 0.0F)
					{
						slopes[1 + k] = hs / (w1 + dzzdx * (hs / del[k]));
					}
				}
			}
		}
	}

	slopes[0] = exteriorSlope(del[0], del[1], h[0], h[1]);
	slopes[7] = exteriorSlope(del[6], del[5], h[6], h[5]);
	for(k = 0; k < 7; k++)
	{
		dzzdx = (del[k] - slopes[k]) / h[k];
		hs = (slopes[k + 1] - del[k]) / h[k];
		pp_coefs[k] = (hs - dzzdx) / h[k];
		pp_coefs[k + 7] = 2.0F * dzzdx - hs;
		pp_coefs[k + 14] = slopes[k];
		pp_coefs[k + 21] = y[k];
	}

	for(k = 0; k < 256; k++)
	{
		low_i = 0;
		low_ip1 = 2;
		high_i = 8;
		while(high_i > low_ip1)
		{
			mid_i = ((low_i + high_i) + 1) >> 1;
			if(((float)k + 1.0F) - 1.0F >= x[mid_i - 1])
			{
				low_i = mid_i - 1;
				low_ip1 = mid_i + 1;
			}
			else
			{
				high_i = mid_i;
			}
		}

		dzzdx = (((float)k + 1.0F) - 1.0F) - x[low_i];
		p[k] = dzzdx * (dzzdx * (dzzdx * pp_coefs[low_i] + pp_coefs[low_i + 7]) +
						pp_coefs[low_i + 14]) + pp_coefs[low_i + 21];
	}
}

QList<int> interpolator(
	const int posX1, const int posX2, const int posX3, const int posX4,
	const int posX5, const int posX6,
	const int posY0, const int posY1, const int posY2, const int posY3,
	const int posY4, const int posY5, const int posY6, const int posY7)
{
	float x[8] = {0.0, static_cast<float>(posX1), static_cast<float>(posX2),
				  static_cast<float>(posX3), static_cast<float>(posX4), static_cast<float>(posX5),
				  static_cast<float>(posX6), 255.0};

	float y[8] = {static_cast<float>(posY0), static_cast<float>(posY1), static_cast<float>(posY2),
				  static_cast<float>(posY3), static_cast<float>(posY4), static_cast<float>(posY5),
				  static_cast<float>(posY6), static_cast<float>(posY7)};

	float p[256];

	pchipInterp(x, y, p);

	int pInt[256];
	for(int i = 0; i < 256; i++)
	{
		pInt[i] = static_cast<int>(p[i]);
	}

	std::list<int> pList;
	for(int i: pInt)
	{
		pList.push_back(i);
	}

	QList<int> interpolatedData = QList<int>::fromStdList(pList);

	return interpolatedData;
}

#endif //PCHIPINTERP_H