private double GetInterpolatedTableValue(byte[] table, int[] xaxis, int[] yaxis, int yvalue, int xvalue)
{
int m_yindex = 0;
int m_xindex = 0;
double result = 0;
double m_ydiff = 0;
double m_xdiff = 0;
double m_ypercentage = 0;
double m_xpercentage = 0;
try
{
for (int yindex = 0; yindex < yaxis.Length; yindex++)
{
if (yvalue > Convert.ToInt32(yaxis.GetValue(yindex)))
{
m_yindex = yindex;
m_ydiff = Math.Abs(Convert.ToDouble(yaxis.GetValue(yindex)) - yvalue);
if (m_yindex < yaxis.Length - 1)
{
m_ypercentage = (double)m_ydiff / (Convert.ToInt32(yaxis.GetValue(yindex + 1)) - Convert.ToInt32(yaxis.GetValue(yindex)));
}
else
{
m_ypercentage = 0;
}
// break;
}
}
for (int xindex = 0; xindex < xaxis.Length; xindex++)
{
if (xvalue > Convert.ToDouble(xaxis.GetValue(xindex)))
{
m_xindex = xindex;
m_xdiff = Math.Abs(Convert.ToDouble(xaxis.GetValue(xindex)) - xvalue);
if (m_xindex < xaxis.Length - 1)
{
m_xpercentage = m_xdiff / (Convert.ToInt32(xaxis.GetValue(xindex + 1)) - Convert.ToInt32(xaxis.GetValue(xindex)));
}
else
{
m_xpercentage = 0;
}
// break;
}
}
//AddDebugLog("RPMindex = " + m_rpmindex + " Percentage = " + m_rpmpercentage.ToString() + " MAPindex = " + m_mapindex.ToString() + " Percentage = " + m_mappercentage.ToString());
// now we found the indexes of the smaller values
byte a1 = 0;
byte a2 = 0;
byte b1 = 0;
byte b2 = 0;
if (m_yindex == (yaxis.Length - 1) && (m_xindex < xaxis.Length - 1))
{
// last row in table, extend with the same values
a1 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex);
a2 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex + 1);
b1 = a1;
b2 = a2;
}
else if (m_yindex == (yaxis.Length - 1) && (m_xindex == xaxis.Length - 1))
{
a1 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex);
a2 = a1;
b1 = a1;
b2 = a1;
return Convert.ToDouble(a1);
}
else if (m_yindex <= (yaxis.Length - 1) && (m_xindex == xaxis.Length - 1))
{
a1 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex);
a2 = a1;
b1 = (byte)table.GetValue(((m_yindex + 1) * xaxis.Length) + m_xindex);
b2 = b1;
}
else
{
a1 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex);
a2 = (byte)table.GetValue((m_yindex * xaxis.Length) + m_xindex + 1);
b1 = (byte)table.GetValue(((m_yindex + 1) * xaxis.Length) + m_xindex);
b2 = (byte)table.GetValue(((m_yindex + 1) * xaxis.Length) + m_xindex + 1);
}
//AddDebugLog("a1 = " + a1.ToString() + " a2 = " + a2.ToString() + " b1 = " + b1.ToString() + " b2 = " + b2.ToString());
// now interpolate the values found
double aval = 0;
double bval = 0;
double adiff = Math.Abs((double)a1 - (double)a2);
if (a1 > a2) aval = a1 - (m_xpercentage * adiff);
else aval = a1 + (m_xpercentage * adiff);
double bdiff = Math.Abs((double)b1 - (double)b2);
if (b1 > b2) bval = b1 - (m_xpercentage * bdiff);
else bval = b1 + (m_xpercentage * bdiff);
// now interpolate vertically (RPM axis)
//AddDebugLog("aval = " + aval.ToString() + " bval = " + bval.ToString());
double abdiff = Math.Abs(aval - bval);
if (aval > bval) result = aval - (m_ypercentage * abdiff);
else result = aval + (m_ypercentage * abdiff);
//AddDebugLog("result = " + result.ToString());
}
catch (Exception E)
{
Console.WriteLine("Failed to interpolate: " + E.Message);
}
return result;
}