/**
 * @copyright (C) 2017 Melexis N.V.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include "mlx90640_API.h"
#include "MLX90640_I2C_Driver.h"
#include <math.h>

int MLX90640::mlx90640_DumpEE(uint8_t slaveAddr, uint16_t *eeData)
{
     return mlx90640_I2CRead(slaveAddr, mlx90640_EEPROM_START_ADDRESS, mlx90640_EEPROM_DUMP_NUM, eeData);
}

void MLX90640::setStubMode(const bool& val)
{
       if (m_stubMode != val) {
           m_stubMode = val;
           mlx90640_I2CSetStub(val);
           emit stubModeChanged();
       }
}

int MLX90640::mlx90640_SynchFrame(uint8_t slaveAddr)
{
    uint16_t dataReady = 0;
    uint16_t statusRegister;
    int error = 1;

    error = mlx90640_I2CWrite(slaveAddr, mlx90640_STATUS_REG, mlx90640_INIT_STATUS_VALUE);
    if(error == -mlx90640_I2C_NACK_ERROR)
    {
        return error;
    }

    while(dataReady == 0)
    {
        error = mlx90640_I2CRead(slaveAddr, mlx90640_STATUS_REG, 1, &statusRegister);
        if(error != mlx90640_NO_ERROR)
        {
            return error;
        }
        //dataReady = statusRegister & 0x0008;
        dataReady = mlx90640_GET_DATA_READY(statusRegister);
    }

   return mlx90640_NO_ERROR;
}

int MLX90640::mlx90640_TriggerMeasurement(uint8_t slaveAddr)
{
    int error = 1;
    uint16_t ctrlReg;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &ctrlReg);

    if ( error != mlx90640_NO_ERROR)
    {
        return error;
    }

    ctrlReg |= mlx90640_CTRL_TRIG_READY_MASK;
    error = mlx90640_I2CWrite(slaveAddr, mlx90640_CTRL_REG, ctrlReg);

    if ( error != mlx90640_NO_ERROR)
    {
        return error;
    }

    error = mlx90640_I2CGeneralReset();

    if ( error != mlx90640_NO_ERROR)
    {
        return error;
    }

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &ctrlReg);

    if ( error != mlx90640_NO_ERROR)
    {
        return error;
    }

    if ((ctrlReg & mlx90640_CTRL_TRIG_READY_MASK) != 0)
    {
        return -mlx90640_MEAS_TRIGGER_ERROR;
    }

    return mlx90640_NO_ERROR;
}

int MLX90640::mlx90640_GetFrameData(uint8_t slaveAddr, uint16_t *frameData)
{
    uint16_t dataReady = 0;
    uint16_t controlRegister1;
    uint16_t statusRegister;
    int error = 1;
    uint16_t data[64];
    uint8_t cnt = 0;

    while(dataReady == 0)
    {
        error = mlx90640_I2CRead(slaveAddr, mlx90640_STATUS_REG, 1, &statusRegister);
        if(error != mlx90640_NO_ERROR)
        {
            return error;
        }
        //dataReady = statusRegister & 0x0008;
        dataReady = mlx90640_GET_DATA_READY(statusRegister);
    }

    error = mlx90640_I2CWrite(slaveAddr, mlx90640_STATUS_REG, mlx90640_INIT_STATUS_VALUE);
    if(error == -mlx90640_I2C_NACK_ERROR)
    {
        return error;
    }

    error = mlx90640_I2CRead(slaveAddr, mlx90640_PIXEL_DATA_START_ADDRESS, mlx90640_PIXEL_NUM, frameData);
    if(error != mlx90640_NO_ERROR)
    {
        return error;
    }

    error = mlx90640_I2CRead(slaveAddr, mlx90640_AUX_DATA_START_ADDRESS, mlx90640_AUX_NUM, data);
    if(error != mlx90640_NO_ERROR)
    {
        return error;
    }

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);
    frameData[832] = controlRegister1;
    //frameData[833] = statusRegister & 0x0001;
    frameData[833] = mlx90640_GET_FRAME(statusRegister);

    if(error != mlx90640_NO_ERROR)
    {
        return error;
    }

    error = ValidateAuxData(data);
    if(error == mlx90640_NO_ERROR)
    {
        for(cnt=0; cnt<mlx90640_AUX_NUM; cnt++)
        {
            frameData[cnt+mlx90640_PIXEL_NUM] = data[cnt];
        }
    }

    error = ValidateFrameData(frameData);
    if (error != mlx90640_NO_ERROR)
    {
        return error;
    }

    return frameData[833];
}

int MLX90640::ValidateFrameData(uint16_t *frameData)
{
    uint8_t line = 0;

    for(int i=0; i<mlx90640_PIXEL_NUM; i+=mlx90640_LINE_SIZE)
    {
        if((frameData[i] == 0x7FFF) && (line%2 == frameData[833])) return -mlx90640_FRAME_DATA_ERROR;
        line = line + 1;
    }

    return mlx90640_NO_ERROR;
}

int MLX90640::ValidateAuxData(uint16_t *auxData)
{

    if(auxData[0] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;

    for(int i=8; i<19; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    for(int i=20; i<23; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    for(int i=24; i<33; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    for(int i=40; i<51; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    for(int i=52; i<55; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    for(int i=56; i<64; i++)
    {
        if(auxData[i] == 0x7FFF) return -mlx90640_FRAME_DATA_ERROR;
    }

    return mlx90640_NO_ERROR;

}

int MLX90640::mlx90640_ExtractParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int error = 0;

    ExtractVDDParameters(eeData, MLX90640);
    ExtractPTATParameters(eeData, MLX90640);
    ExtractGainParameters(eeData, MLX90640);
    ExtractTgcParameters(eeData, MLX90640);
    ExtractResolutionParameters(eeData, MLX90640);
    ExtractKsTaParameters(eeData, MLX90640);
    ExtractKsToParameters(eeData, MLX90640);
    ExtractCPParameters(eeData, MLX90640);
    ExtractAlphaParameters(eeData, MLX90640);
    ExtractOffsetParameters(eeData, MLX90640);
    ExtractKtaPixelParameters(eeData, MLX90640);
    ExtractKvPixelParameters(eeData, MLX90640);
    ExtractCILCParameters(eeData, MLX90640);
    error = ExtractDeviatingPixels(eeData, MLX90640);

    return error;

}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_SetResolution(uint8_t slaveAddr, uint8_t resolution)
{
    uint16_t controlRegister1;
    uint16_t value;
    int error;

    //value = (resolution & 0x03) << 10;
    value = ((uint16_t)resolution << mlx90640_CTRL_RESOLUTION_SHIFT);
    value &= ~mlx90640_CTRL_RESOLUTION_MASK;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);

    if(error == mlx90640_NO_ERROR)
    {
        value = (controlRegister1 & mlx90640_CTRL_RESOLUTION_MASK) | value;
        error = mlx90640_I2CWrite(slaveAddr, mlx90640_CTRL_REG, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_GetCurResolution(uint8_t slaveAddr)
{
    uint16_t controlRegister1;
    int resolutionRAM;
    int error;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);
    if(error != mlx90640_NO_ERROR)
    {
        return error;
    }
    resolutionRAM = (controlRegister1 & ~mlx90640_CTRL_RESOLUTION_MASK) >> mlx90640_CTRL_RESOLUTION_SHIFT;

    return resolutionRAM;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_SetRefreshRate(uint8_t slaveAddr, uint8_t refreshRate)
{
    uint16_t controlRegister1;
    uint16_t value;
    int error;

    //value = (refreshRate & 0x07)<<7;
    value = ((uint16_t)refreshRate << mlx90640_CTRL_REFRESH_SHIFT);
    value &= ~mlx90640_CTRL_REFRESH_MASK;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);
    if(error == mlx90640_NO_ERROR)
    {
        value = (controlRegister1 & mlx90640_CTRL_REFRESH_MASK) | value;
        error = mlx90640_I2CWrite(slaveAddr, mlx90640_CTRL_REG, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_GetRefreshRate(uint8_t slaveAddr)
{
    uint16_t controlRegister1;
    int refreshRate;
    int error;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);
    if(error != mlx90640_NO_ERROR)
    {
        return error;
    }
    refreshRate = (controlRegister1 & ~mlx90640_CTRL_REFRESH_MASK) >> mlx90640_CTRL_REFRESH_SHIFT;

    return refreshRate;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_SetInterleavedMode(uint8_t slaveAddr)
{
    uint16_t controlRegister1;
    uint16_t value;
    int error;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);

    if(error == 0)
    {
        value = (controlRegister1 & ~mlx90640_CTRL_MEAS_MODE_MASK);
        error = mlx90640_I2CWrite(slaveAddr, mlx90640_CTRL_REG, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_SetChessMode(uint8_t slaveAddr)
{
    uint16_t controlRegister1;
    uint16_t value;
    int error;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);

    if(error == 0)
    {
        value = (controlRegister1 | mlx90640_CTRL_MEAS_MODE_MASK);
        error = mlx90640_I2CWrite(slaveAddr, mlx90640_CTRL_REG, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_GetCurMode(uint8_t slaveAddr)
{
    uint16_t controlRegister1;
    int modeRAM;
    int error;

    error = mlx90640_I2CRead(slaveAddr, mlx90640_CTRL_REG, 1, &controlRegister1);
    if(error != 0)
    {
        return error;
    }
    modeRAM = (controlRegister1 & mlx90640_CTRL_MEAS_MODE_MASK) >> mlx90640_CTRL_MEAS_MODE_SHIFT;

    return modeRAM;
}

//------------------------------------------------------------------------------

void MLX90640::mlx90640_CalculateTo(uint16_t *frameData, const paramsMLX90640 *params, float emissivity, float tr, float *result)
{
    float vdd;
    float ta;
    float ta4;
    float tr4;
    float taTr;
    float gain;
    float irDataCP[2];
    float irData;
    float alphaCompensated;
    uint8_t mode;
    int8_t ilPattern;
    int8_t chessPattern;
    int8_t pattern;
    int8_t conversionPattern;
    float Sx;
    float To;
    float alphaCorrR[4];
    int8_t range;
    uint16_t subPage;
    float ktaScale;
    float kvScale;
    float alphaScale;
    float kta;
    float kv;

    subPage = frameData[833];
    vdd = mlx90640_GetVdd(frameData, params);
    ta = mlx90640_GetTa(frameData, params);

    ta4 = (ta + 273.15);
    ta4 = ta4 * ta4;
    ta4 = ta4 * ta4;
    tr4 = (tr + 273.15);
    tr4 = tr4 * tr4;
    tr4 = tr4 * tr4;
    taTr = tr4 - (tr4-ta4)/emissivity;

    ktaScale = POW2(params->ktaScale);
    kvScale = POW2(params->kvScale);
    alphaScale = POW2(params->alphaScale);

    alphaCorrR[0] = 1 / (1 + params->ksTo[0] * 40);
    alphaCorrR[1] = 1 ;
    alphaCorrR[2] = (1 + params->ksTo[1] * params->ct[2]);
    alphaCorrR[3] = alphaCorrR[2] * (1 + params->ksTo[2] * (params->ct[3] - params->ct[2]));

//------------------------- Gain calculation -----------------------------------

    gain = (float)params->gainEE / (int16_t)frameData[778];

//------------------------- To calculation -------------------------------------
    mode = (frameData[832] & mlx90640_CTRL_MEAS_MODE_MASK) >> 5;

    irDataCP[0] = (int16_t)frameData[776] * gain;
    irDataCP[1] = (int16_t)frameData[808] * gain;

    irDataCP[0] = irDataCP[0] - params->cpOffset[0] * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
    if( mode ==  params->calibrationModeEE)
    {
        irDataCP[1] = irDataCP[1] - params->cpOffset[1] * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
    }
    else
    {
      irDataCP[1] = irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
    }

    for( int pixelNumber = 0; pixelNumber < 768; pixelNumber++)
    {
        ilPattern = pixelNumber / 32 - (pixelNumber / 64) * 2;
        chessPattern = ilPattern ^ (pixelNumber - (pixelNumber/2)*2);
        conversionPattern = ((pixelNumber + 2) / 4 - (pixelNumber + 3) / 4 + (pixelNumber + 1) / 4 - pixelNumber / 4) * (1 - 2 * ilPattern);

        if(mode == 0)
        {
          pattern = ilPattern;
        }
        else
        {
          pattern = chessPattern;
        }

        if(pattern == frameData[833])
        {
            irData = (int16_t)frameData[pixelNumber] * gain;

            kta = params->kta[pixelNumber]/ktaScale;
            kv = params->kv[pixelNumber]/kvScale;
            irData = irData - params->offset[pixelNumber]*(1 + kta*(ta - 25))*(1 + kv*(vdd - 3.3));

            if(mode !=  params->calibrationModeEE)
            {
              irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) - params->ilChessC[1] * conversionPattern;
            }

            irData = irData - params->tgc * irDataCP[subPage];
            irData = irData / emissivity;

            alphaCompensated = SCALEALPHA*alphaScale/params->alpha[pixelNumber];
            alphaCompensated = alphaCompensated*(1 + params->KsTa * (ta - 25));

            Sx = alphaCompensated * alphaCompensated * alphaCompensated * (irData + alphaCompensated * taTr);
            Sx = sqrt(sqrt(Sx)) * params->ksTo[1];

            To = sqrt(sqrt(irData/(alphaCompensated * (1 - params->ksTo[1] * 273.15) + Sx) + taTr)) - 273.15;

            if(To < params->ct[1])
            {
                range = 0;
            }
            else if(To < params->ct[2])
            {
                range = 1;
            }
            else if(To < params->ct[3])
            {
                range = 2;
            }
            else
            {
                range = 3;
            }

            To = sqrt(sqrt(irData / (alphaCompensated * alphaCorrR[range] * (1 + params->ksTo[range] * (To - params->ct[range]))) + taTr)) - 273.15;

            result[pixelNumber] = To;
        }
    }
}

//------------------------------------------------------------------------------

void MLX90640::mlx90640_GetImage(uint16_t *frameData, const paramsMLX90640 *params, float *result)
{
    float vdd;
        float ta;
        float gain;
        float irDataCP[2];
        float irData;
        float alphaCompensated;
        uint8_t mode;
        int8_t ilPattern;
        int8_t chessPattern;
        int8_t pattern;
        int8_t conversionPattern;
        float image;
        uint16_t subPage;
        float ktaScale;
        float kvScale;
        float kta;
        float kv;

        subPage = frameData[833];
        vdd = mlx90640_GetVdd(frameData, params);
        ta = mlx90640_GetTa(frameData, params);

        ktaScale = POW2(params->ktaScale);
        kvScale = POW2(params->kvScale);

    //------------------------- Gain calculation -----------------------------------

        gain = (float)params->gainEE / (int16_t)frameData[778];

    //------------------------- Image calculation -------------------------------------

        mode = (frameData[832] & mlx90640_CTRL_MEAS_MODE_MASK) >> 5;

        irDataCP[0] = (int16_t)frameData[776] * gain;
        irDataCP[1] = (int16_t)frameData[808] * gain;

        irDataCP[0] = irDataCP[0] - params->cpOffset[0] * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
        if( mode ==  params->calibrationModeEE)
        {
            irDataCP[1] = irDataCP[1] - params->cpOffset[1] * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
        }
        else
        {
          irDataCP[1] = irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) * (1 + params->cpKta * (ta - 25)) * (1 + params->cpKv * (vdd - 3.3));
        }

        for( int pixelNumber = 0; pixelNumber < 768; pixelNumber++)
        {
            ilPattern = pixelNumber / 32 - (pixelNumber / 64) * 2;
            chessPattern = ilPattern ^ (pixelNumber - (pixelNumber/2)*2);
            conversionPattern = ((pixelNumber + 2) / 4 - (pixelNumber + 3) / 4 + (pixelNumber + 1) / 4 - pixelNumber / 4) * (1 - 2 * ilPattern);

            if(mode == 0)
            {
              pattern = ilPattern;
            }
            else
            {
              pattern = chessPattern;
            }

            if(pattern == frameData[833])
            {
                irData = (int16_t)frameData[pixelNumber] * gain;

                kta = params->kta[pixelNumber]/ktaScale;
                kv = params->kv[pixelNumber]/kvScale;
                irData = irData - params->offset[pixelNumber]*(1 + kta*(ta - 25))*(1 + kv*(vdd - 3.3));

                if(mode !=  params->calibrationModeEE)
                {
                  irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) - params->ilChessC[1] * conversionPattern;
                }

                irData = irData - params->tgc * irDataCP[subPage];

                alphaCompensated = params->alpha[pixelNumber];

                image = irData*alphaCompensated;

                result[pixelNumber] = image;
            }
        }
}

//------------------------------------------------------------------------------

float MLX90640::mlx90640_GetVdd(uint16_t *frameData, const paramsMLX90640 *params)
{
    float vdd;
    float resolutionCorrection;

    uint16_t resolutionRAM;

    resolutionRAM = (frameData[832] & ~mlx90640_CTRL_RESOLUTION_MASK) >> mlx90640_CTRL_RESOLUTION_SHIFT;
    resolutionCorrection = POW2(params->resolutionEE) / POW2(resolutionRAM);
    vdd = (resolutionCorrection * (int16_t)frameData[810] - params->vdd25) / params->kVdd + 3.3;

    return vdd;
}

//------------------------------------------------------------------------------

float MLX90640::mlx90640_GetTa(uint16_t *frameData, const paramsMLX90640 *params)
{
    int16_t ptat;
    float ptatArt;
    float vdd;
    float ta;

    vdd = mlx90640_GetVdd(frameData, params);

    ptat = (int16_t)frameData[800];

    ptatArt = (ptat / (ptat * params->alphaPTAT + (int16_t)frameData[768])) * POW2(18);

    ta = (ptatArt / (1 + params->KvPTAT * (vdd - 3.3)) - params->vPTAT25);
    ta = ta / params->KtPTAT + 25;

    return ta;
}

//------------------------------------------------------------------------------

int MLX90640::mlx90640_GetSubPageNumber(uint16_t *frameData)
{
    return frameData[833];

}

//------------------------------------------------------------------------------
void MLX90640::mlx90640_BadPixelsCorrection(uint16_t *pixels, float *to, int mode, paramsMLX90640 *params)
{
    float ap[4];
    uint8_t pix;
    uint8_t line;
    uint8_t column;

    pix = 0;
    while(pixels[pix] != 0xFFFF)
    {
        line = pixels[pix]>>5;
        column = pixels[pix] - (line<<5);

        if(mode == 1)
        {
            if(line == 0)
            {
                if(column == 0)
                {
                    to[pixels[pix]] = to[33];
                }
                else if(column == 31)
                {
                    to[pixels[pix]] = to[62];
                }
                else
                {
                    to[pixels[pix]] = (to[pixels[pix]+31] + to[pixels[pix]+33])/2.0;
                }
            }
            else if(line == 23)
            {
                if(column == 0)
                {
                    to[pixels[pix]] = to[705];
                }
                else if(column == 31)
                {
                    to[pixels[pix]] = to[734];
                }
                else
                {
                    to[pixels[pix]] = (to[pixels[pix]-33] + to[pixels[pix]-31])/2.0;
                }
            }
            else if(column == 0)
            {
                to[pixels[pix]] = (to[pixels[pix]-31] + to[pixels[pix]+33])/2.0;
            }
            else if(column == 31)
            {
                to[pixels[pix]] = (to[pixels[pix]-33] + to[pixels[pix]+31])/2.0;
            }
            else
            {
                ap[0] = to[pixels[pix]-33];
                ap[1] = to[pixels[pix]-31];
                ap[2] = to[pixels[pix]+31];
                ap[3] = to[pixels[pix]+33];
                to[pixels[pix]] = GetMedian(ap,4);
            }
        }
        else
        {
            if(column == 0)
            {
                to[pixels[pix]] = to[pixels[pix]+1];
            }
            else if(column == 1 || column == 30)
            {
                to[pixels[pix]] = (to[pixels[pix]-1]+to[pixels[pix]+1])/2.0;
            }
            else if(column == 31)
            {
                to[pixels[pix]] = to[pixels[pix]-1];
            }
            else
            {
                if(IsPixelBad(pixels[pix]-2,params) == 0 && IsPixelBad(pixels[pix]+2,params) == 0)
                {
                    ap[0] = to[pixels[pix]+1] - to[pixels[pix]+2];
                    ap[1] = to[pixels[pix]-1] - to[pixels[pix]-2];
                    if(fabs(ap[0]) > fabs(ap[1]))
                    {
                        to[pixels[pix]] = to[pixels[pix]-1] + ap[1];
                    }
                    else
                    {
                        to[pixels[pix]] = to[pixels[pix]+1] + ap[0];
                    }
                }
                else
                {
                    to[pixels[pix]] = (to[pixels[pix]-1]+to[pixels[pix]+1])/2.0;
                }
            }
        }
        pix = pix + 1;
    }
}

//------------------------------------------------------------------------------

void MLX90640::ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int8_t kVdd;
    int16_t vdd25;

    kVdd = mlx90640_MS_BYTE(eeData[51]);

    vdd25 = mlx90640_LS_BYTE(eeData[51]);
    vdd25 = ((vdd25 - 256) << 5) - 8192;

    MLX90640->kVdd = 32 * kVdd;
    MLX90640->vdd25 = vdd25;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractPTATParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    float KvPTAT;
    float KtPTAT;
    int16_t vPTAT25;
    float alphaPTAT;

    KvPTAT = (eeData[50] & mlx90640_MSBITS_6_MASK) >> 10;
    if(KvPTAT > 31)
    {
        KvPTAT = KvPTAT - 64;
    }
    KvPTAT = KvPTAT/4096;

    KtPTAT = eeData[50] & mlx90640_LSBITS_10_MASK;
    if(KtPTAT > 511)
    {
        KtPTAT = KtPTAT - 1024;
    }
    KtPTAT = KtPTAT/8;

    vPTAT25 = eeData[49];

    alphaPTAT = (eeData[16] & mlx90640_NIBBLE4_MASK) / POW2(14) + 8.0f;

    MLX90640->KvPTAT = KvPTAT;
    MLX90640->KtPTAT = KtPTAT;
    MLX90640->vPTAT25 = vPTAT25;
    MLX90640->alphaPTAT = alphaPTAT;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractGainParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    MLX90640->gainEE = (int16_t)eeData[48];;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractTgcParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    MLX90640->tgc = (int8_t)mlx90640_LS_BYTE(eeData[60]) / 32.0f;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractResolutionParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    uint8_t resolutionEE;
    resolutionEE = (eeData[56] & 0x3000) >> 12;

    MLX90640->resolutionEE = resolutionEE;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractKsTaParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    MLX90640->KsTa = (int8_t)mlx90640_MS_BYTE(eeData[60]) / 8192.0f;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractKsToParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int32_t KsToScale;
    int8_t step;

    step = ((eeData[63] & 0x3000) >> 12) * 10;

    MLX90640->ct[0] = -40;
    MLX90640->ct[1] = 0;
    MLX90640->ct[2] = mlx90640_NIBBLE2(eeData[63]);
    MLX90640->ct[3] = mlx90640_NIBBLE3(eeData[63]);

    MLX90640->ct[2] = MLX90640->ct[2]*step;
    MLX90640->ct[3] = MLX90640->ct[2] + MLX90640->ct[3]*step;
    MLX90640->ct[4] = 400;

    KsToScale = mlx90640_NIBBLE1(eeData[63]) + 8;
    KsToScale = 1UL << KsToScale;

    MLX90640->ksTo[0] = (int8_t)mlx90640_LS_BYTE(eeData[61]) / (float)KsToScale;
    MLX90640->ksTo[1] = (int8_t)mlx90640_MS_BYTE(eeData[61]) / (float)KsToScale;
    MLX90640->ksTo[2] = (int8_t)mlx90640_LS_BYTE(eeData[62]) / (float)KsToScale;
    MLX90640->ksTo[3] = (int8_t)mlx90640_MS_BYTE(eeData[62]) / (float)KsToScale;
    MLX90640->ksTo[4] = -0.0002;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractAlphaParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int accRow[24];
    int accColumn[32];
    int p = 0;
    int alphaRef;
    uint8_t alphaScale;
    uint8_t accRowScale;
    uint8_t accColumnScale;
    uint8_t accRemScale;
    float alphaTemp[768];
    float temp;


    accRemScale = mlx90640_NIBBLE1(eeData[32]);
    accColumnScale = mlx90640_NIBBLE2(eeData[32]);
    accRowScale = mlx90640_NIBBLE3(eeData[32]);
    alphaScale = mlx90640_NIBBLE4(eeData[32]) + 30;
    alphaRef = eeData[33];

    for(int i = 0; i < 6; i++)
    {
        p = i * 4;
        accRow[p + 0] = mlx90640_NIBBLE1(eeData[34 + i]);
        accRow[p + 1] = mlx90640_NIBBLE2(eeData[34 + i]);
        accRow[p + 2] = mlx90640_NIBBLE3(eeData[34 + i]);
        accRow[p + 3] = mlx90640_NIBBLE4(eeData[34 + i]);
    }

    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        if (accRow[i] > 7)
        {
            accRow[i] = accRow[i] - 16;
        }
    }

    for(int i = 0; i < 8; i++)
    {
        p = i * 4;
        accColumn[p + 0] = mlx90640_NIBBLE1(eeData[40 + i]);
        accColumn[p + 1] = mlx90640_NIBBLE2(eeData[40 + i]);
        accColumn[p + 2] = mlx90640_NIBBLE3(eeData[40 + i]);
        accColumn[p + 3] = mlx90640_NIBBLE4(eeData[40 + i]);
    }

    for(int i = 0; i < mlx90640_COLUMN_NUM; i++)
    {
        if (accColumn[i] > 7)
        {
            accColumn[i] = accColumn[i] - 16;
        }
    }

    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        for(int j = 0; j < mlx90640_COLUMN_NUM; j ++)
        {
            p = 32 * i +j;
            alphaTemp[p] = (eeData[64 + p] & 0x03F0) >> 4;
            if (alphaTemp[p] > 31)
            {
                alphaTemp[p] = alphaTemp[p] - 64;
            }
            alphaTemp[p] = alphaTemp[p]*(1 << accRemScale);
            alphaTemp[p] = (alphaRef + (accRow[i] << accRowScale) + (accColumn[j] << accColumnScale) + alphaTemp[p]);
            alphaTemp[p] = alphaTemp[p] / POW2(alphaScale);
            alphaTemp[p] = alphaTemp[p] - MLX90640->tgc * (MLX90640->cpAlpha[0] + MLX90640->cpAlpha[1])/2;
            alphaTemp[p] = SCALEALPHA/alphaTemp[p];
        }
    }

    temp = alphaTemp[0];
    for(int i = 1; i < mlx90640_PIXEL_NUM; i++)
    {
        if (alphaTemp[i] > temp)
        {
            temp = alphaTemp[i];
        }
    }

    alphaScale = 0;
    while(temp < 32767.4)
    {
        temp = temp*2;
        alphaScale = alphaScale + 1;
    }

    for(int i = 0; i < mlx90640_PIXEL_NUM; i++)
    {
        temp = alphaTemp[i] * POW2(alphaScale);
        MLX90640->alpha[i] = (temp + 0.5);

    }

    MLX90640->alphaScale = alphaScale;

}

//------------------------------------------------------------------------------

void MLX90640::ExtractOffsetParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int occRow[24];
    int occColumn[32];
    int p = 0;
    int16_t offsetRef;
    uint8_t occRowScale;
    uint8_t occColumnScale;
    uint8_t occRemScale;


    occRemScale = mlx90640_NIBBLE1(eeData[16]);
    occColumnScale = mlx90640_NIBBLE2(eeData[16]);
    occRowScale = mlx90640_NIBBLE3(eeData[16]);
    offsetRef = (int16_t)eeData[17];

    for(int i = 0; i < 6; i++)
    {
        p = i * 4;
        occRow[p + 0] = mlx90640_NIBBLE1(eeData[18 + i]);
        occRow[p + 1] = mlx90640_NIBBLE2(eeData[18 + i]);
        occRow[p + 2] = mlx90640_NIBBLE3(eeData[18 + i]);
        occRow[p + 3] = mlx90640_NIBBLE4(eeData[18 + i]);
    }

    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        if (occRow[i] > 7)
        {
            occRow[i] = occRow[i] - 16;
        }
    }

    for(int i = 0; i < 8; i++)
    {
        p = i * 4;
        occColumn[p + 0] = mlx90640_NIBBLE1(eeData[24 + i]);
        occColumn[p + 1] = mlx90640_NIBBLE2(eeData[24 + i]);
        occColumn[p + 2] = mlx90640_NIBBLE3(eeData[24 + i]);
        occColumn[p + 3] = mlx90640_NIBBLE4(eeData[24 + i]);
    }

    for(int i = 0; i < mlx90640_COLUMN_NUM; i ++)
    {
        if (occColumn[i] > 7)
        {
            occColumn[i] = occColumn[i] - 16;
        }
    }

    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        for(int j = 0; j < mlx90640_COLUMN_NUM; j ++)
        {
            p = 32 * i +j;
            MLX90640->offset[p] = (eeData[64 + p] & mlx90640_MSBITS_6_MASK) >> 10;
            if (MLX90640->offset[p] > 31)
            {
                MLX90640->offset[p] = MLX90640->offset[p] - 64;
            }
            MLX90640->offset[p] = MLX90640->offset[p]*(1 << occRemScale);
            MLX90640->offset[p] = (offsetRef + (occRow[i] << occRowScale) + (occColumn[j] << occColumnScale) + MLX90640->offset[p]);
        }
    }
}

//------------------------------------------------------------------------------

void MLX90640::ExtractKtaPixelParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int p = 0;
    int8_t KtaRC[4];
    uint8_t ktaScale1;
    uint8_t ktaScale2;
    uint8_t split;
    float ktaTemp[768];
    float temp;

    KtaRC[0] = (int8_t)mlx90640_MS_BYTE(eeData[54]);;
    KtaRC[2] = (int8_t)mlx90640_LS_BYTE(eeData[54]);;
    KtaRC[1] = (int8_t)mlx90640_MS_BYTE(eeData[55]);;
    KtaRC[3] = (int8_t)mlx90640_LS_BYTE(eeData[55]);;

    ktaScale1 = mlx90640_NIBBLE2(eeData[56]) + 8;
    ktaScale2 = mlx90640_NIBBLE1(eeData[56]);

    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        for(int j = 0; j < mlx90640_COLUMN_NUM; j ++)
        {
            p = 32 * i +j;
            split = 2*(p/32 - (p/64)*2) + p%2;
            ktaTemp[p] = (eeData[64 + p] & 0x000E) >> 1;
            if (ktaTemp[p] > 3)
            {
                ktaTemp[p] = ktaTemp[p] - 8;
            }
            ktaTemp[p] = ktaTemp[p] * (1 << ktaScale2);
            ktaTemp[p] = KtaRC[split] + ktaTemp[p];
            ktaTemp[p] = ktaTemp[p] / POW2(ktaScale1);

        }
    }

    temp = fabs(ktaTemp[0]);
    for(int i = 1; i < mlx90640_PIXEL_NUM; i++)
    {
        if (fabs(ktaTemp[i]) > temp)
        {
            temp = fabs(ktaTemp[i]);
        }
    }

    ktaScale1 = 0;
    while(temp < 63.4)
    {
        temp = temp*2;
        ktaScale1 = ktaScale1 + 1;
    }

    for(int i = 0; i < mlx90640_PIXEL_NUM; i++)
    {
        temp = ktaTemp[i] * POW2(ktaScale1);
        if (temp < 0)
        {
            MLX90640->kta[i] = (temp - 0.5);
        }
        else
        {
            MLX90640->kta[i] = (temp + 0.5);
        }

    }

    MLX90640->ktaScale = ktaScale1;
}


//------------------------------------------------------------------------------

void MLX90640::ExtractKvPixelParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    int p = 0;
    int8_t KvT[4];
    int8_t KvRoCo;
    int8_t KvRoCe;
    int8_t KvReCo;
    int8_t KvReCe;
    uint8_t kvScale;
    uint8_t split;
    float kvTemp[768];
    float temp;

    KvRoCo = mlx90640_NIBBLE4(eeData[52]);
    if (KvRoCo > 7)
    {
        KvRoCo = KvRoCo - 16;
    }
    KvT[0] = KvRoCo;

    KvReCo = mlx90640_NIBBLE3(eeData[52]);
    if (KvReCo > 7)
    {
        KvReCo = KvReCo - 16;
    }
    KvT[2] = KvReCo;

    KvRoCe = mlx90640_NIBBLE2(eeData[52]);
    if (KvRoCe > 7)
    {
        KvRoCe = KvRoCe - 16;
    }
    KvT[1] = KvRoCe;

    KvReCe = mlx90640_NIBBLE1(eeData[52]);
    if (KvReCe > 7)
    {
        KvReCe = KvReCe - 16;
    }
    KvT[3] = KvReCe;

    kvScale = mlx90640_NIBBLE3(eeData[56]);


    for(int i = 0; i < mlx90640_LINE_NUM; i++)
    {
        for(int j = 0; j < mlx90640_COLUMN_NUM; j ++)
        {
            p = 32 * i +j;
            split = 2*(p/32 - (p/64)*2) + p%2;
            kvTemp[p] = KvT[split];
            kvTemp[p] = kvTemp[p] / POW2(kvScale);
        }
    }

    temp = fabs(kvTemp[0]);
    for(int i = 1; i < mlx90640_PIXEL_NUM; i++)
    {
        if (fabs(kvTemp[i]) > temp)
        {
            temp = fabs(kvTemp[i]);
        }
    }

    kvScale = 0;
    while(temp < 63.4)
    {
        temp = temp*2;
        kvScale = kvScale + 1;
    }

    for(int i = 0; i < mlx90640_PIXEL_NUM; i++)
    {
        temp = kvTemp[i] * POW2(kvScale);
        if (temp < 0)
        {
            MLX90640->kv[i] = (temp - 0.5);
        }
        else
        {
            MLX90640->kv[i] = (temp + 0.5);
        }

    }

    MLX90640->kvScale = kvScale;
}

//------------------------------------------------------------------------------

void MLX90640::ExtractCPParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    float alphaSP[2];
    int16_t offsetSP[2];
    float cpKv;
    float cpKta;
    uint8_t alphaScale;
    uint8_t ktaScale1;
    uint8_t kvScale;

    alphaScale = mlx90640_NIBBLE4(eeData[32]) + 27;

    offsetSP[0] = (eeData[58] & mlx90640_LSBITS_10_MASK);
    if (offsetSP[0] > 511)
    {
        offsetSP[0] = offsetSP[0] - 1024;
    }

    offsetSP[1] = (eeData[58] & mlx90640_MSBITS_6_MASK) >> 10;
    if (offsetSP[1] > 31)
    {
        offsetSP[1] = offsetSP[1] - 64;
    }
    offsetSP[1] = offsetSP[1] + offsetSP[0];

    alphaSP[0] = (eeData[57] & mlx90640_LSBITS_10_MASK);
    if (alphaSP[0] > 511)
    {
        alphaSP[0] = alphaSP[0] - 1024;
    }
    alphaSP[0] = alphaSP[0] /  POW2(alphaScale);

    alphaSP[1] = (eeData[57] & mlx90640_MSBITS_6_MASK) >> 10;
    if (alphaSP[1] > 31)
    {
        alphaSP[1] = alphaSP[1] - 64;
    }
    alphaSP[1] = (1 + alphaSP[1]/128) * alphaSP[0];

    cpKta = (int8_t)mlx90640_LS_BYTE(eeData[59]);

    ktaScale1 = mlx90640_NIBBLE2(eeData[56]) + 8;
    MLX90640->cpKta = cpKta / POW2(ktaScale1);

    cpKv = (int8_t)mlx90640_MS_BYTE(eeData[59]);

    kvScale = mlx90640_NIBBLE3(eeData[56]);
    MLX90640->cpKv = cpKv / POW2(kvScale);

    MLX90640->cpAlpha[0] = alphaSP[0];
    MLX90640->cpAlpha[1] = alphaSP[1];
    MLX90640->cpOffset[0] = offsetSP[0];
    MLX90640->cpOffset[1] = offsetSP[1];
}

//------------------------------------------------------------------------------

void MLX90640::ExtractCILCParameters(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    float ilChessC[3];
    uint8_t calibrationModeEE;

    calibrationModeEE = (eeData[10] & 0x0800) >> 4;
    calibrationModeEE = calibrationModeEE ^ 0x80;

    ilChessC[0] = (eeData[53] & 0x003F);
    if (ilChessC[0] > 31)
    {
        ilChessC[0] = ilChessC[0] - 64;
    }
    ilChessC[0] = ilChessC[0] / 16.0f;

    ilChessC[1] = (eeData[53] & 0x07C0) >> 6;
    if (ilChessC[1] > 15)
    {
        ilChessC[1] = ilChessC[1] - 32;
    }
    ilChessC[1] = ilChessC[1] / 2.0f;

    ilChessC[2] = (eeData[53] & 0xF800) >> 11;
    if (ilChessC[2] > 15)
    {
        ilChessC[2] = ilChessC[2] - 32;
    }
    ilChessC[2] = ilChessC[2] / 8.0f;

    MLX90640->calibrationModeEE = calibrationModeEE;
    MLX90640->ilChessC[0] = ilChessC[0];
    MLX90640->ilChessC[1] = ilChessC[1];
    MLX90640->ilChessC[2] = ilChessC[2];
}

//------------------------------------------------------------------------------

int MLX90640::ExtractDeviatingPixels(uint16_t *eeData, paramsMLX90640 *MLX90640)
{
    uint16_t pixCnt = 0;
    uint16_t brokenPixCnt = 0;
    uint16_t outlierPixCnt = 0;
    int warn = 0;
    int i;

    for(pixCnt = 0; pixCnt<5; pixCnt++)
    {
        MLX90640->brokenPixels[pixCnt] = 0xFFFF;
        MLX90640->outlierPixels[pixCnt] = 0xFFFF;
    }

    pixCnt = 0;
    while (pixCnt < mlx90640_PIXEL_NUM && brokenPixCnt < 5 && outlierPixCnt < 5)
    {
        if(eeData[pixCnt+64] == 0)
        {
            MLX90640->brokenPixels[brokenPixCnt] = pixCnt;
            brokenPixCnt = brokenPixCnt + 1;
        }
        else if((eeData[pixCnt+64] & 0x0001) != 0)
        {
            MLX90640->outlierPixels[outlierPixCnt] = pixCnt;
            outlierPixCnt = outlierPixCnt + 1;
        }

        pixCnt = pixCnt + 1;

    }

    if(brokenPixCnt > 4)
    {
        warn = -mlx90640_BROKEN_PIXELS_NUM_ERROR;
    }
    else if(outlierPixCnt > 4)
    {
        warn = -mlx90640_OUTLIER_PIXELS_NUM_ERROR;
    }
    else if((brokenPixCnt + outlierPixCnt) > 4)
    {
        warn = -mlx90640_BAD_PIXELS_NUM_ERROR;
    }
    else
    {
        for(pixCnt=0; pixCnt<brokenPixCnt; pixCnt++)
        {
            for(i=pixCnt+1; i<brokenPixCnt; i++)
            {
                warn = CheckAdjacentPixels(MLX90640->brokenPixels[pixCnt],MLX90640->brokenPixels[i]);
                if(warn != 0)
                {
                    return warn;
                }
            }
        }

        for(pixCnt=0; pixCnt<outlierPixCnt; pixCnt++)
        {
            for(i=pixCnt+1; i<outlierPixCnt; i++)
            {
                warn = CheckAdjacentPixels(MLX90640->outlierPixels[pixCnt],MLX90640->outlierPixels[i]);
                if(warn != 0)
                {
                    return warn;
                }
            }
        }

        for(pixCnt=0; pixCnt<brokenPixCnt; pixCnt++)
        {
            for(i=0; i<outlierPixCnt; i++)
            {
                warn = CheckAdjacentPixels(MLX90640->brokenPixels[pixCnt],MLX90640->outlierPixels[i]);
                if(warn != 0)
                {
                    return warn;
                }
            }
        }

    }


    return warn;

}

//------------------------------------------------------------------------------

 int MLX90640::CheckAdjacentPixels(uint16_t pix1, uint16_t pix2)
 {

     int pixPosDif;
     uint16_t lp1 = pix1 >> 5;
     uint16_t lp2 = pix2 >> 5;
     uint16_t cp1 = pix1 - (lp1 << 5);
     uint16_t cp2 = pix2 - (lp2 << 5);

     pixPosDif = lp1 - lp2;
     if(pixPosDif > -2 && pixPosDif < 2)
     {
        pixPosDif = cp1 - cp2;
        if(pixPosDif > -2 && pixPosDif < 2)
        {
            return -6;
        }

     }

     return 0;
 }

//------------------------------------------------------------------------------

float MLX90640::GetMedian(float *values, int n)
 {
    float temp;

    for(int i=0; i<n-1; i++)
    {
        for(int j=i+1; j<n; j++)
        {
            if(values[j] < values[i])
            {
                temp = values[i];
                values[i] = values[j];
                values[j] = temp;
            }
        }
    }

    if(n%2==0)
    {
        return ((values[n/2] + values[n/2 - 1]) / 2.0);

    }
    else
    {
        return values[n/2];
    }

 }

//------------------------------------------------------------------------------

int MLX90640::IsPixelBad(uint16_t pixel,paramsMLX90640 *params)
{
    for(int i=0; i<5; i++)
    {
        if(pixel == params->outlierPixels[i] || pixel == params->brokenPixels[i])
        {
            return 1;
        }
    }

    return 0;
}

//------------------------------------------------------------------------------
