/** @file  psMatrixVectorArithmetic.c
 *
 *  @brief Provides unary and binary functions for simple matrix and vector element operations. Functions
 *  include:
 *
 *      Addition (+)
 *      Subtraction (-)
 *      Multiplication (*)
 *      Division (/)
 *      Power (^)
 *      Minimum (min)
 *      Maximum (max)
 *      Absolute value (abs)
 *      Exponent (exp)
 *      Natural Log (ln)
 *      Power of 10 (ten)
 *      Log (log)
 *      Sine (sin or dsin)
 *      Cosine (cos or dcos)
 *      Tangent (tan or dtan)
 *      Arcsine (asin or dasin)
 *      Arccosine (acos or dacos)
 *      Arctan (atan or datan)
 *
 *  Currently only vector-vector and image-image binary operations are supported.
 *
 *  @ingroup MatrixVectorArithmetic
 *
 *  @author Ross Harman, MHPCC
 *
 *  @version $Revision: 1.34 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-01-12 22:17:01 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 */

/******************************************************************************
 *  INCLUDE FILES                                                             *
 ******************************************************************************/
#include <string.h>
#include <math.h>
#include <stdint.h>

#include "psMemory.h"
#include "psError.h"
#include "psImage.h"
#include "psVector.h"
#include "psScalar.h"
#include "psLogMsg.h"
#include "psConstants.h"
#include "psDataManipErrors.h"

/*****************************************************************************
 *  FUNCTION IMPLEMENTATION - LOCAL                                          *
 *****************************************************************************/

// Conversion for degrees to radians
#define D2R 0.01745329252111111  /* PI/180 */

// Conversion for radians to degrees
#define R2D 57.29577950924861   /* 180.0/PI */

// Binary SCALAR_XXXX operations
#define SCALAR_SCALAR(OUT,IN1,OP,IN2,TYPE)                                                                   \
{                                                                                                            \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    o  = &((psScalar* )OUT)->data.TYPE;                                                                      \
    i1 = &((psScalar* )IN1)->data.TYPE;                                                                      \
    i2 = &((psScalar* )IN2)->data.TYPE;                                                                      \
    *o = OP;                                                                                                 \
}

#define SCALAR_VECTOR(OUT,IN1,OP,IN2,TYPE)                                                                   \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 npt = 0;                                                                                             \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    npt  = ((psVector* )IN2)->n;                                                                             \
    o  = ((psVector* )OUT)->data.TYPE;                                                                       \
    i1 = &((psScalar* )IN1)->data.TYPE;                                                                      \
    i2 = ((psVector* )IN2)->data.TYPE;                                                                       \
    for (i=0; i < npt; i++, o++, i2++) {                                                                     \
        *o = OP;                                                                                             \
    }                                                                                                        \
}

#define SCALAR_IMAGE(OUT,IN1,OP,IN2,TYPE)                                                                    \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 j = 0;                                                                                               \
    psS32 numRows = 0;                                                                                         \
    psS32 numCols = 0;                                                                                         \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    numRows = ((psImage* )IN2)->numRows;                                                                     \
    numCols = ((psImage* )IN2)->numCols;                                                                     \
    for(j = 0; j < numCols; j++) {                                                                           \
        o  = ((psImage* )OUT)->data.TYPE[j];                                                                 \
        i1 = &((psScalar* )IN1)->data.TYPE;                                                                  \
        i2 = ((psImage* )IN2)->data.TYPE[j];                                                                 \
        for(i = 0; i < numRows; i++, o++, i2++) {                                                            \
            *o = OP;                                                                                         \
        }                                                                                                    \
    }                                                                                                        \
}

// Binary VECTOR_XXXX operations
#define VECTOR_SCALAR(OUT,IN1,OP,IN2,TYPE)                                                                   \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 n1 = 0;                                                                                              \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    n1  = ((psVector* )IN1)->n;                                                                              \
    o  = ((psVector* )OUT)->data.TYPE;                                                                       \
    i1 = ((psVector* )IN1)->data.TYPE;                                                                       \
    i2 = &((psScalar* )IN2)->data.TYPE;                                                                      \
    for (i=0; i < n1; i++, o++, i1++) {                                                                      \
        *o = OP;                                                                                             \
    }                                                                                                        \
}

#define VECTOR_VECTOR(OUT,IN1,OP,IN2,TYPE)                                                                   \
{                                                                                                            \
    psS32 i = 0;                                                                                             \
    psS32 n1 = 0;                                                                                            \
    psS32 n2 = 0;                                                                                            \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    n1  = ((psVector* )IN1)->n;                                                                              \
    n2  = ((psVector* )IN2)->n;                                                                              \
    if(n1 != n2) {                                                                                           \
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                             \
                PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                         \
                n1, n2);                                                                                     \
        if (OUT != IN1 && OUT != IN2) {                                                                      \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    }                                                                                                        \
    o  = ((psVector* )OUT)->data.TYPE;                                                                       \
    i1 = ((psVector* )IN1)->data.TYPE;                                                                       \
    i2 = ((psVector* )IN2)->data.TYPE;                                                                       \
    for (i=0; i < n1; i++, o++, i1++, i2++) {                                                                \
        *o = OP;                                                                                             \
    }                                                                                                        \
}

#define VECTOR_IMAGE(OUT,IN1,OP,IN2,TYPE)                                                                    \
{                                                                                                            \
    psS32 i = 0;                                                                                             \
    psS32 j = 0;                                                                                             \
    psS32 n1 = 0;                                                                                            \
    psS32 numRows2 = 0;                                                                                      \
    psS32 numCols2 = 0;                                                                                      \
    psDimen dim1 = 0;                                                                                        \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    n1  = ((psVector* )IN1)->n;                                                                              \
    dim1 = ((psVector* )IN1)->type.dimen;                                                                    \
    numRows2 = ((psImage* )IN2)->numRows;                                                                    \
    numCols2 = ((psImage* )IN2)->numCols;                                                                    \
    \
    if(dim1 == PS_DIMEN_VECTOR) { /* Regular vectors */                                             \
        if(n1!=numRows2) {                                                                                   \
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                         \
                    PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                           \
                    n1, numRows2);                                                                           \
            if (OUT != IN1 && OUT != IN2) {                                                                  \
                psFree(OUT);                                                                                 \
            }                                                                                                \
            return NULL;                                                                                     \
        }                                                                                                    \
        \
        i1 = ((psVector* )IN1)->data.TYPE;                                                                   \
        for(j = 0; j < numRows2; j++, i1++) {                                                                \
            o  = ((psImage* )OUT)->data.TYPE[j];                                                             \
            i2 = ((psImage* )IN2)->data.TYPE[j];                                                             \
            for(i = 0; i < numCols2; i++, o++, i2++) {                                                       \
                *o = OP;                                                                                     \
            }                                                                                                \
        }                                                                                                    \
    } else {  /* Transposed vectors */                                                                       \
        if(n1!=numCols2) {                                                                                   \
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                 \
                    PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                     \
                    n1, numCols2);                       \
            if (OUT != IN1 && OUT != IN2) {                                                                  \
                psFree(OUT);                                                                                 \
            }                                                                                                \
            return NULL;                                                                                     \
        }                                                                                                    \
        \
        for(j = 0; j < numRows2; j++) {                                                                      \
            o  = ((psImage* )OUT)->data.TYPE[j];                                                             \
            i1 = ((psVector* )IN1)->data.TYPE;                                                               \
            i2 = ((psImage* )IN2)->data.TYPE[j];                                                             \
            for(i = 0; i < numCols2; i++, o++, i1++, i2++) {                                                 \
                *o = OP;                                                                                     \
            }                                                                                                \
        }                                                                                                    \
    }                                                                                                        \
}

// Binary IMAGE_XXXX operations
#define IMAGE_SCALAR(OUT,IN1,OP,IN2,TYPE)                                                                    \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 j = 0;                                                                                               \
    psS32 numRows = 0;                                                                                         \
    psS32 numCols = 0;                                                                                         \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    numRows = ((psImage* )IN1)->numRows;                                                                     \
    numCols = ((psImage* )IN1)->numCols;                                                                     \
    for(j = 0; j < numRows; j++) {                                                                           \
        o  = ((psImage* )OUT)->data.TYPE[j];                                                                 \
        i1 = ((psImage* )IN1)->data.TYPE[j];                                                                 \
        i2 = &((psScalar* )IN2)->data.TYPE;                                                                  \
        for(i = 0; i < numCols; i++, o++, i1++) {                                                            \
            *o = OP;                                                                                         \
        }                                                                                                    \
    }                                                                                                        \
}

#define IMAGE_VECTOR(OUT,IN1,OP,IN2,TYPE)                                                                    \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 j = 0;                                                                                               \
    psS32 n2 = 0;                                                                                              \
    psS32 numRows1 = 0;                                                                                        \
    psS32 numCols1 = 0;                                                                                        \
    psDimen dim2 = 0;                                                                                        \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    n2  = ((psVector* )IN2)->n;                                                                              \
    dim2 = ((psVector* )IN2)->type.dimen;                                                                    \
    numRows1 = ((psImage* )IN1)->numRows;                                                                    \
    numCols1 = ((psImage* )IN1)->numCols;                                                                    \
    \
    if(dim2 == PS_DIMEN_VECTOR) { /* Regular vectors */                                             \
        if(n2!=numRows1) {                                                                                   \
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                         \
                    PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                     \
                    n2, numRows1);                                                                           \
            if (OUT != IN1 && OUT != IN2) {                                                                  \
                psFree(OUT);                                                                                 \
            }                                                                                                \
            return NULL;                                                                                     \
        }                                                                                                    \
        \
        i2 = ((psVector* )IN2)->data.TYPE;                                                                   \
        for(j = 0; j < numRows1; j++, i1++) {                                                                \
            o  = ((psImage* )OUT)->data.TYPE[j];                                                             \
            i1 = ((psImage* )IN1)->data.TYPE[j];                                                             \
            for(i = 0; i < numCols1; i++, o++, i1++) {                                                       \
                *o = OP;                                                                                     \
            }                                                                                                \
        }                                                                                                    \
    } else {  /* Transposed vectors */                                                                       \
        if(n2!=numCols1) {                                                                                   \
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                         \
                    PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                     \
                    n2, numCols1);                                                                           \
            if (OUT != IN1) {                                                                                \
                psFree(OUT);                                                                                 \
            }                                                                                                \
            return NULL;                                                                                     \
        }                                                                                                    \
        \
        for(j = 0; j < numRows1; j++) {                                                                      \
            o  = ((psImage* )OUT)->data.TYPE[j];                                                             \
            i1 = ((psVector* )IN2)->data.TYPE;                                                               \
            i2 = ((psImage* )IN1)->data.TYPE[j];                                                             \
            for(i = 0; i < numCols1; i++, o++, i2++, i1++) {                                                 \
                *o = OP;                                                                                     \
            }                                                                                                \
        }                                                                                                    \
    }                                                                                                        \
}

#define IMAGE_IMAGE(OUT,IN1,OP,IN2,TYPE)                                                                     \
{                                                                                                            \
    psS32 i = 0;                                                                                               \
    psS32 j = 0;                                                                                               \
    psS32 numRows1 = 0;                                                                                        \
    psS32 numCols1 = 0;                                                                                        \
    psS32 numRows2 = 0;                                                                                        \
    psS32 numCols2 = 0;                                                                                        \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    numRows1 = ((psImage* )IN1)->numRows;                                                                    \
    numCols1 = ((psImage* )IN1)->numCols;                                                                    \
    numRows2 = ((psImage* )IN2)->numRows;                                                                    \
    numCols2 = ((psImage* )IN2)->numCols;                                                                    \
    if(numRows1!=numRows2 || numCols1!=numCols2) {                                                           \
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                             \
                PS_ERRORTEXT_psMatrix_IMAGE_SIZE_DIFFERS,                                                     \
                numCols1, numRows1, numCols2, numRows2);                                                     \
        if (OUT != IN1 && OUT != IN2) {                                                                      \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    }                                                                                                        \
    for(j = 0; j < numRows1; j++) {                                                                          \
        o  = ((psImage* )OUT)->data.TYPE[j];                                                                 \
        i1 = ((psImage* )IN1)->data.TYPE[j];                                                                 \
        i2 = ((psImage* )IN2)->data.TYPE[j];                                                                 \
        for(i = 0; i < numCols1; i++, o++, i1++, i2++) {                                                     \
            *o = OP;                                                                                         \
        }                                                                                                    \
    }                                                                                                        \
}

// Preprocessor macro function to create arithmetic function based on input type
#define BINARY_TYPE(DIM1,DIM2,OUT,IN1,OP,IN2)                                                                \
switch (IN1->type) {                                                                                         \
case PS_TYPE_U8:                                                                                             \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,U8);                                                                        \
    break;                                                                                                   \
case PS_TYPE_U16:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,U16);                                                                       \
    break;                                                                                                   \
case PS_TYPE_U32:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,U32);                                                                       \
    break;                                                                                                   \
case PS_TYPE_U64:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,U64);                                                                       \
    break;                                                                                                   \
case PS_TYPE_S8:                                                                                             \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,S8);                                                                        \
    break;                                                                                                   \
case PS_TYPE_S16:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,S16);                                                                       \
    break;                                                                                                   \
case PS_TYPE_S32:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,S32);                                                                       \
    break;                                                                                                   \
case PS_TYPE_S64:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,S64);                                                                       \
    break;                                                                                                   \
case PS_TYPE_F32:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,F32);                                                                       \
    break;                                                                                                   \
case PS_TYPE_F64:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,F64);                                                                       \
    break;                                                                                                   \
case PS_TYPE_C32:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,C32);                                                                       \
    break;                                                                                                   \
case PS_TYPE_C64:                                                                                            \
    DIM1##_##DIM2(OUT,IN1,OP,IN2,C64);                                                                       \
    break;                                                                                                   \
default:                                                                                                     \
    /* char* strType; \
    PS_TYPE_NAME(strType,IN1->type);                                                                         \
    psError(PS_ERR_BAD_PARAMETER_TYPE, true,                                                                 \
            PS_ERRORTEXT_psMatrix_TYPE_MISMATCH,                                                             \
            strType);  */                                                                                      \
    if (OUT != IN1 && OUT != IN2) {                                                                          \
        psFree(OUT);                                                                                         \
    }                                                                                                        \
    return NULL;                                                                                             \
}

// Preprocessor macro function to create arithmetic function operation name
#define BINARY_OP(DIM1,DIM2,OUT,IN1,OP,IN2)                                                                  \
if(!strncmp(OP, "=", 1)) {                                                                                   \
    BINARY_TYPE(DIM1,DIM2,OUT,IN1,*i2,IN2);                                                                  \
} else if(!strncmp(OP, "+", 1)) {                                                                            \
    BINARY_TYPE(DIM1,DIM2,OUT,IN1,*i1 + *i2,IN2);                                                            \
} else if(!strncmp(OP, "-", 1)) {                                                                            \
    BINARY_TYPE(DIM1,DIM2,OUT,IN1,*i1 - *i2,IN2);                                                            \
} else if(!strncmp(OP, "*", 1)) {                                                                            \
    BINARY_TYPE(DIM1,DIM2,OUT,IN1,*i1 * *i2,IN2);                                                            \
} else if(!strncmp(OP, "/", 1)) {                                                                            \
    BINARY_TYPE(DIM1,DIM2,OUT,IN1,*i1 / *i2,IN2);                                                            \
} else if(!strncmp(OP, "^", 1)) {                                                                            \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN1->type)) {                                                                \
        BINARY_TYPE(DIM1,DIM2,OUT,IN1,cpow(*i1,*i2),IN2);                                                    \
    } else {                                                                                                 \
        BINARY_TYPE(DIM1,DIM2,OUT,IN1,pow(*i1,*i2),IN2);                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "min", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN1->type)) {                                                                \
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,                                                            \
                PS_ERRORTEXT_psMatrix_MIN_COMPLEX_SUPPORT);                                                  \
        if (OUT != IN1 && OUT != IN2) {                                                                      \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    } else {                                                                                                 \
        BINARY_TYPE(DIM1,DIM2,OUT,IN1,fmin(*i1,*i2),IN2);                                                    \
    }                                                                                                        \
} else if(!strncmp(OP, "max", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN1->type)) {                                                                \
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,                                                            \
                PS_ERRORTEXT_psMatrix_MAX_COMPLEX_SUPPORT);                                                  \
        if (OUT != IN1 && OUT != IN2) {                                                                      \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    } else {                                                                                                 \
        BINARY_TYPE(DIM1,DIM2,OUT,IN1,fmax(*i1,*i2),IN2);                                                    \
    }                                                                                                        \
} else {                                                                                                     \
    psError(PS_ERR_BAD_PARAMETER_VALUE, true,                                                                \
            PS_ERRORTEXT_psMatrix_OPERATION_UNSUPPORTED,                                                     \
            OP);                                                                                             \
    if (OUT != IN1 && OUT != IN2) {                                                                          \
        psFree(OUT);                                                                                         \
    }                                                                                                        \
    return NULL;                                                                                             \
}

psPtr psBinaryOp(psPtr out, psPtr in1, char *op, psPtr in2)
{

    psVector* input1 = (psVector* ) in1;
    psVector* input2 = (psVector* ) in2;

    #define psBinaryOp_EXIT { \
                              if (out != in1 && out != in2) { \
                              psFree(out); \
                              } \
                              return NULL; \
                            }

    PS_PTR_CHECK_NULL_GENERAL(input1, psBinaryOp_EXIT);
    PS_PTR_CHECK_NULL_GENERAL(input2, psBinaryOp_EXIT);
    PS_PTR_CHECK_NULL_GENERAL(op, psBinaryOp_EXIT);

    PS_PTR_CHECK_TYPE_EQUAL_GENERAL(input1,input2, psBinaryOp_EXIT);

    PS_PTR_CHECK_DIMEN_GENERAL_NOT(input1, PS_DIMEN_OTHER, psBinaryOp_EXIT);
    PS_PTR_CHECK_DIMEN_GENERAL_NOT(input2, PS_DIMEN_OTHER, psBinaryOp_EXIT);

    psType* psType1 = (psType*)in1;
    psType* psType2 = (psType*)in2;
    psDimen dim1 = psType1->dimen;
    psDimen dim2 = psType2->dimen;
    psElemType elType1 = psType1->type;
    psElemType elType2 = psType2->type;

    if (dim1 == PS_DIMEN_VECTOR || dim1 == PS_DIMEN_TRANSV) {
        if (((psVector* ) in1)->n == 0) {
            psLogMsg(__func__, PS_LOG_WARN, "Vector contains zero elements");
        }
    } else if (dim1 == PS_DIMEN_IMAGE) {
        if (((psImage* ) in1)->numCols == 0 || ((psImage* ) in1)->numRows == 0) {
            psLogMsg(__func__, PS_LOG_WARN, "Image contains zero length row or cols");
        }
    }

    if (dim2 == PS_DIMEN_VECTOR || dim2 == PS_DIMEN_TRANSV) {
        if (((psVector* ) in2)->n == 0) {
            psLogMsg(__func__, PS_LOG_WARN, "Vector contains zero elements");
        }
    } else if (dim2 == PS_DIMEN_IMAGE) {
        if (((psImage* ) in2)->numCols == 0 || ((psImage* ) in2)->numRows == 0) {
            psLogMsg(__func__, PS_LOG_WARN, "Image contains zero length row or cols");
        }
    }

    if (dim1 == PS_DIMEN_SCALAR) {
        if ( out != NULL && ((psType*)out)->dimen != dim2) {
            if (out != in1 && out != in2) {
                psFree(out);
            }
            out = NULL;
        }
        if (dim2 == PS_DIMEN_SCALAR) {
            if (out == NULL || ((psScalar*)out)->type.type != elType1) {
                if (out != in1 && out != in2) {
                    psFree(out);
                }
                out = psScalarAlloc(0.0,elType1);
            }
            BINARY_OP(SCALAR, SCALAR, out, psType1, op, psType2);       // scalar op scalar
        } else if (dim2 == PS_DIMEN_VECTOR || dim2 == PS_DIMEN_TRANSV) {
            out = psVectorRecycle(out,((psVector*)in2)->n,elType1);
            if (out == NULL) {
                psError(PS_ERR_UNKNOWN, false,
                        PS_ERRORTEXT_psMatrix_OUTPUT_VECTOR_NOT_CREATED);
                return NULL;
            }
            BINARY_OP(SCALAR, VECTOR, out, psType1, op, psType2);       // scalar op vector
        } else if (dim2 == PS_DIMEN_IMAGE) {
            out = psImageRecycle(out, ((psImage* ) in2)->numCols, ((psImage* ) in2)->numRows,elType1);
            if (out == NULL) {
                psError(PS_ERR_UNKNOWN, false,
                        PS_ERRORTEXT_psMatrix_OUTPUT_IMAGE_NOT_CREATED);
                return NULL;
            }
            BINARY_OP(SCALAR, IMAGE, out, psType1, op, psType2);        // scalar op image
        } else {
            psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                    PS_ERRORTEXT_psMatrix_DIMEN_INVALID,
                    "in2",dim2);
            psBinaryOp_EXIT;
        }
    } else if (dim1 == PS_DIMEN_VECTOR || dim1 == PS_DIMEN_TRANSV) {
        if (dim2 == PS_DIMEN_SCALAR) {
            out = psVectorRecycle(out,((psVector*)in1)->n,elType1);
            if (out == NULL) {
                psError(PS_ERR_UNKNOWN, false,
                        PS_ERRORTEXT_psMatrix_OUTPUT_VECTOR_NOT_CREATED);
                return NULL;
            }
            BINARY_OP(VECTOR, SCALAR, out, psType1, op, psType2);       // vector op scalar
        } else if (dim2 == PS_DIMEN_VECTOR || dim2 == PS_DIMEN_TRANSV) {
            out = psVectorRecycle(out,((psVector*)in2)->n,elType2);
            if (out == NULL) {
                psError(PS_ERR_UNKNOWN, false,
                        PS_ERRORTEXT_psMatrix_OUTPUT_VECTOR_NOT_CREATED);
                return NULL;
            }
            BINARY_OP(VECTOR, VECTOR, out, psType1, op, psType2);       // vector op vector
        } else if (dim2 == PS_DIMEN_IMAGE) {
            out = psImageRecycle(out, ((psImage* ) in2)->numCols, ((psImage* ) in2)->numRows, elType2);
            if (out == NULL) {
                psError(PS_ERR_UNKNOWN, false,
                        PS_ERRORTEXT_psMatrix_OUTPUT_IMAGE_NOT_CREATED);
                return NULL;
            }
            BINARY_OP(VECTOR, IMAGE, out, psType1, op, psType2);        // vector op image
        } else {
            psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                    PS_ERRORTEXT_psMatrix_DIMEN_INVALID,
                    "in2",dim2);
            psBinaryOp_EXIT;
        }
    } else if (dim1 == PS_DIMEN_IMAGE) {
        out = psImageRecycle(out, ((psImage*)in1)->numCols, ((psImage*)in1)->numRows, elType1);
        if (out == NULL) {
            psError(PS_ERR_UNKNOWN, false,
                    PS_ERRORTEXT_psMatrix_OUTPUT_IMAGE_NOT_CREATED);
            return NULL;
        }
        if (dim2 == PS_DIMEN_SCALAR) {
            BINARY_OP(IMAGE, SCALAR, out, psType1, op, psType2);        // image op scalar
        } else if (dim2 == PS_DIMEN_VECTOR || dim2 == PS_DIMEN_TRANSV) {
            BINARY_OP(IMAGE, VECTOR, out, psType1, op, psType2);        // image op vector
        } else if (dim2 == PS_DIMEN_IMAGE) {
            BINARY_OP(IMAGE, IMAGE, out, psType1, op, psType2); // image op image
        } else {
            psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                    PS_ERRORTEXT_psMatrix_DIMEN_INVALID,
                    "in2",dim2);
            psBinaryOp_EXIT;
        }
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psMatrix_DIMEN_INVALID,
                "in1",dim1);
        psBinaryOp_EXIT;
    }

    // Automtically free psScalar types, since they are usually allocated in the argument list when this
    // function is called, provided that the input is not the output.
    if(psType1->dimen==PS_DIMEN_SCALAR && in1!=out) {
        psFree(in1);
    }

    if(psType2->dimen==PS_DIMEN_SCALAR && in2!=out) {
        psFree(in2);
    }

    return out;
}

// Unary SCALAR operations
#define SCALAR(OUT,IN,OP,TYPE)                                                                               \
{                                                                                                            \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    o  = &((psScalar* )OUT)->data.TYPE;                                                                      \
    i1 = &((psScalar* )IN)->data.TYPE;                                                                       \
    *o = OP;                                                                                                 \
}

// Unary IMAGE operations
#define VECTOR(OUT,IN,OP,TYPE)                                                                               \
{                                                                                                            \
    psS32 i = 0;                                                                                             \
    psS32 nIn = 0;                                                                                           \
    psS32 nOut = 0;                                                                                          \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    nIn = ((psVector* )IN)->n;                                                                               \
    nOut = ((psVector* )OUT)->n;                                                                             \
    if(nIn != nOut) {                                                                                        \
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                             \
                PS_ERRORTEXT_psMatrix_COUNT_DIFFERS,                                                         \
                nIn, nOut);                                                                                  \
        if (OUT != IN) {                                                                                     \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    }                                                                                                        \
    o  = ((psVector* )OUT)->data.TYPE;                                                                       \
    i1 = ((psVector* )IN)->data.TYPE;                                                                        \
    for(i = 0; i < nIn; i++, o++, i1++) {                                                                    \
        *o = OP;                                                                                             \
    }                                                                                                        \
}

// Unary IMAGE operations
#define IMAGE(OUT,IN,OP,TYPE)                                                                                \
{                                                                                                            \
    psS32 i = 0;                                                                                             \
    psS32 j = 0;                                                                                             \
    psS32 numRowsIn = 0;                                                                                     \
    psS32 numColsIn = 0;                                                                                     \
    psS32 numRowsOut = 0;                                                                                    \
    psS32 numColsOut = 0;                                                                                    \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    numRowsIn = ((psImage* )IN)->numRows;                                                                    \
    numColsIn = ((psImage* )IN)->numCols;                                                                    \
    numRowsOut = ((psImage* )OUT)->numRows;                                                                  \
    numColsOut = ((psImage* )OUT)->numCols;                                                                  \
    if(numRowsIn!=numRowsOut || numColsIn!=numColsOut) {                                                     \
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,                                                             \
                PS_ERRORTEXT_psMatrix_IMAGE_SIZE_DIFFERS,                                                    \
                numColsIn, numRowsIn, numColsOut, numRowsOut);                                               \
        if (OUT != IN) {                                                                                     \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    }                                                                                                        \
    for(j = 0; j < numRowsIn; j++) {                                                                         \
        o  = ((psImage* )OUT)->data.TYPE[j];                                                                 \
        i1 = ((psImage* )IN)->data.TYPE[j];                                                                  \
        for(i = 0; i < numColsIn; i++, o++, i1++) {                                                          \
            *o = OP;                                                                                         \
        }                                                                                                    \
    }                                                                                                        \
}

// Preprocessor macro function to create arithmetic function based on input type
#define UNARY_TYPE(DIM,OUT,IN,OP)                                                                            \
switch (IN->type) {                                                                                          \
case PS_TYPE_S32:                                                                                            \
    DIM(OUT,IN,OP,S32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_F32:                                                                                            \
    DIM(OUT,IN,OP,F32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_F64:                                                                                            \
    DIM(OUT,IN,OP,F64);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_C32:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_S8:                                                                                             \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_U8:                                                                                             \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_S16:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_U16:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_U32:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_S64:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_U64:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
case PS_TYPE_C64:                                                                                            \
    DIM(OUT,IN,OP,C32);                                                                                      \
    break;                                                                                                   \
default: {                                                                                                     \
        char* strType;                                                                                           \
        PS_TYPE_NAME(strType, IN->type);                                                                         \
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,                                                                 \
                PS_ERRORTEXT_psMatrix_TYPE_MISMATCH,                                                             \
                strType);                                                                                        \
        if (OUT != IN) {                                                                                         \
            psFree(OUT);                                                                                         \
        }                                                                                                        \
        return NULL;                                                                                             \
    } \
}

// Preprocessor macro function to create arithmetic function operation name. Functions below that add
// FLT_EPSILON are done so to align results with a 64 bit computing architecture
#define UNARY_OP(DIM,OUT,IN,OP)                                                                              \
if(!strncmp(OP, "abs", 3)) {                                                                                 \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cabs(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,fabs(*i1));                                                                    \
    }                                                                                                        \
} else if(!strncmp(OP, "exp", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cexp(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,exp(*i1));                                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "ln", 2)) {                                                                           \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,clog(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,log(*i1));                                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "ten", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cpow(10.0,*i1));                                                               \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,pow(10.0,*i1));                                                                \
    }                                                                                                        \
} else if(!strncmp(OP, "log", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,clog(*i1)/log(10.0));                                                          \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,log10(*i1));                                                                   \
    }                                                                                                        \
} else if(!strncmp(OP, "sin", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,csin(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,sin(*i1));                                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "dsin", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,csin(*i1*D2R));                                                                \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,sin(*i1*D2R));                                                                 \
    }                                                                                                        \
} else if(!strncmp(OP, "cos", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,ccos(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cos(*i1));                                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "dcos", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,ccos(*i1*D2R));                                                                \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cos(*i1*D2R));                                                                 \
    }                                                                                                        \
} else if(!strncmp(OP, "tan", 3)) {                                                                          \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,ctan(*i1));                                                                    \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,tan(*i1));                                                                     \
    }                                                                                                        \
} else if(!strncmp(OP, "dtan", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,ctan(*i1*D2R));                                                                \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,tan(*i1*D2R));                                                                 \
    }                                                                                                        \
} else if(!strncmp(OP, "asin", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,casin(*i1));                                                                   \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,asin(*i1));                                                                    \
    }                                                                                                        \
} else if(!strncmp(OP, "dasin", 5)) {                                                                        \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,R2D*casin(*i1));                                                               \
    } else if(PS_IS_PSELEMTYPE_INT(IN->type)) {                                                              \
        UNARY_TYPE(DIM,OUT,IN,(R2D*asin(*i1)));                                                              \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,(R2D*asin(*i1)));                                                              \
    }                                                                                                        \
} else if(!strncmp(OP, "acos", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,cacos(*i1));                                                                   \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,acos(*i1));                                                                    \
    }                                                                                                        \
} else if(!strncmp(OP, "dacos", 5)) {                                                                        \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,R2D*cacos(*i1));                                                               \
    } else if(PS_IS_PSELEMTYPE_INT(IN->type)) {                                                              \
        UNARY_TYPE(DIM,OUT,IN,(R2D*acos(*i1)));                                                              \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,R2D*acos(*i1));                                                                \
    }                                                                                                        \
} else if(!strncmp(OP, "atan", 4)) {                                                                         \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,catan(*i1));                                                                   \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,atan(*i1));                                                                    \
    }                                                                                                        \
} else if(!strncmp(OP, "datan", 5)) {                                                                        \
    if(PS_IS_PSELEMTYPE_COMPLEX(IN->type)) {                                                                 \
        UNARY_TYPE(DIM,OUT,IN,R2D*catan(*i1));                                                               \
    } else {                                                                                                 \
        UNARY_TYPE(DIM,OUT,IN,R2D*atan(*i1));                                                                \
    }                                                                                                        \
} else {                                                                                                     \
    psError(PS_ERR_BAD_PARAMETER_VALUE, true,                                                                \
            PS_ERRORTEXT_psMatrix_OPERATION_UNSUPPORTED,                                                     \
            OP);                                                                                             \
    if (OUT != IN) {                                                                                         \
        psFree(OUT);                                                                                         \
    }                                                                                                        \
    return NULL;                                                                                             \
}

psPtr psUnaryOp(psPtr out, psPtr in, char *op)
{
    #define psUnaryOp_EXIT { \
                             if (out != in) { \
                             psFree(out); \
                             } \
                             return NULL; \
                           }

    psType* psTypeIn = (psType* ) in;

    PS_PTR_CHECK_NULL_GENERAL(in, psUnaryOp_EXIT);
    PS_PTR_CHECK_NULL_GENERAL(op, psUnaryOp_EXIT);

    psDimen dimIn = psTypeIn->dimen;
    psElemType elTypeIn = psTypeIn->type;

    switch (dimIn) {
    case PS_DIMEN_SCALAR:
        if (out == NULL ||
                ((psType*)out)->dimen != PS_DIMEN_SCALAR ||
                ((psScalar*)out)->type.type != elTypeIn) {
            psFree(out);
            out = psScalarAlloc(0.0,elTypeIn);
        }
        UNARY_OP(SCALAR, out, psTypeIn, op);    // scalar
        break;
    case PS_DIMEN_VECTOR:
    case PS_DIMEN_TRANSV:
        if (((psVector*)in)->n == 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    PS_ERRORTEXT_psMatrix_VECTOR_EMPTY);
            psUnaryOp_EXIT;
        }

        out = psVectorRecycle(out,
                              ((psVector*)in)->n,
                              elTypeIn);
        if (out == NULL) {
            psError(PS_ERR_UNKNOWN, false,
                    PS_ERRORTEXT_psMatrix_OUTPUT_VECTOR_NOT_CREATED);
            psUnaryOp_EXIT;
        }

        UNARY_OP(VECTOR, out, psTypeIn, op);    // vector
        break;
    case PS_DIMEN_IMAGE:
        if (((psImage* ) in)->numCols == 0 || ((psImage* ) in)->numRows == 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    PS_ERRORTEXT_psMatrix_IMAGE_EMPTY);
            psUnaryOp_EXIT;
        }

        out = psImageRecycle(out,
                             ((psImage*)in)->numCols,
                             ((psImage*)in)->numRows,
                             elTypeIn);
        if (out == NULL) {
            psError(PS_ERR_UNKNOWN, false,
                    PS_ERRORTEXT_psMatrix_OUTPUT_IMAGE_NOT_CREATED);
            psUnaryOp_EXIT;
        }

        UNARY_OP(IMAGE, out, psTypeIn, op);     // image
        break;
    default:
        if (out != in) {
            psFree(out);
        }
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                PS_ERRORTEXT_psMatrix_DIMEN_INVALID,
                "in", dimIn);
        psUnaryOp_EXIT;
    }

    // Automtically free psScalar types, since they are usually allocated in the argument list when this
    // function is called, provided that the input is not the output.
    if(psTypeIn->dimen==PS_DIMEN_SCALAR && in!=out) {
        psFree(in);
    }

    return out;
}
