/** @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.28 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2004-10-08 02:48: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"

/*****************************************************************************
 *  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 */

/* IEEE Standards on floating-point makes this function pointless, really.
// Division with NAN checking
static complex double psNanDiv(complex double a, complex double b)
{
    complex double out = 0 + 0i;
 
    out = a / b;
    if (isnan(creal(out)) || isnan(cimag(out))) {
        psError(__func__, ": Divide by zero");
    }
 
    return out;
}
*/

// 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)                                                                   \
{                                                                                                            \
    int i = 0;                                                                                               \
    int npt = 0;                                                                                             \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    ps##TYPE *i2 = NULL;                                                                                     \
    npt  = ((psVector* )IN1)->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)                                                                    \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int numRows = 0;                                                                                         \
    int 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 < 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)                                                                   \
{                                                                                                            \
    int i = 0;                                                                                               \
    int 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)                                                                   \
{                                                                                                            \
    int i = 0;                                                                                               \
    int n1 = 0;                                                                                              \
    int 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(__func__, ": Inconsistent element count: %d vs %d", 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)                                                                    \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int n1 = 0;                                                                                              \
    int numRows2 = 0;                                                                                        \
    int 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(__func__, ": Inconsistent element count: %d vs %d", 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(__func__, ": Inconsistent element count: %d vs %d", 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)                                                                    \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int numRows = 0;                                                                                         \
    int 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)                                                                    \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int n2 = 0;                                                                                              \
    int numRows1 = 0;                                                                                        \
    int 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(__func__, ": Inconsistent element count: %d vs %d", 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(__func__, ": Inconsistent element count: %d vs %d", 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)                                                                     \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int numRows1 = 0;                                                                                        \
    int numCols1 = 0;                                                                                        \
    int numRows2 = 0;                                                                                        \
    int 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(__func__, ": Inconsistent element count: numRows: %d vs %d numCols: %d vs %d", numRows1,     \
                numRows2, numCols1, numCols2);                                                               \
        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:                                                                                                     \
    psError(__func__, ": Invalid PS_TYPE: %d", IN1->type);                                                   \
    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,*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(__func__, ": Minimum operation not supported for complex numbers");                          \
        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(__func__, ": Maximum operation not supported for complex numbers");                          \
        if (OUT != IN1 && OUT != IN2) {                                                                      \
            psFree(OUT);                                                                                     \
        }                                                                                                    \
        return NULL;                                                                                         \
    } else {                                                                                                 \
        BINARY_TYPE(DIM1,DIM2,OUT,IN1,fmax(*i1,*i2),IN2);                                                    \
    }                                                                                                        \
} else {                                                                                                     \
    psError(__func__, ": Invalid operation: %s", OP);                                                        \
    if (OUT != IN1 && OUT != IN2) {                                                                          \
        psFree(OUT);                                                                                         \
    }                                                                                                        \
    return NULL;                                                                                             \
}

void *psBinaryOp(void *out, void *in1, char *op, void *in2)
{
    psDimen dim1 = 0;
    psDimen dim2 = 0;
    psElemType elType1 = 0;
    psElemType elType2 = 0;
    psType* psType1 = NULL;
    psType* psType2 = NULL;

    psType1 = (psType* ) in1;
    if (psType1 == NULL) {
        psError(__func__, ": Line %d - Null in1 argument", __LINE__);
        if (out != in1 && out != in2) {
            psFree(out);
        }
        return NULL;
    }

    psType2 = (psType* ) in2;
    if (psType2 == NULL) {
        psError(__func__, ": Line %d - Null in2 argument", __LINE__);
        if (out != in1 && out != in2) {
            psFree(out);
        }
        return NULL;
    }

    if (op == NULL) {
        psError(__func__, ": Line %d - Null op argument", __LINE__);
        if (out != in1 && out != in2) {
            psFree(out);
        }
        return NULL;
    }

    dim1 = psType1->dimen;
    dim2 = psType2->dimen;
    elType1 = psType1->type;
    elType2 = psType2->type;

    if (elType1 != elType2) {
        psError(__func__, ": Line %d - Element types for arguments inconsistent: (%d, %d)", __LINE__,
                elType1, elType2);
        if (out != in1 && out != in2) {
            psFree(out);
        }
        return NULL;
    }

    if (dim1 == PS_DIMEN_OTHER || dim2 == PS_DIMEN_OTHER) {
        psError(__func__, ": Line %d - PS_DIMEN_OTHER not allowed for arguments: (%d, %d)", __LINE__,
                dim1, dim2);
        if (out != in1 && out != in2) {
            psFree(out);
        }
        return NULL;
    }

    if (dim1 == PS_DIMEN_VECTOR || dim1 == PS_DIMEN_TRANSV) {
        if (((psVector* ) in1)->n == 0) {
            psLogMsg(__func__, PS_LOG_WARN, ": Line %d - Vector contains zero elements");
        }
    } else if (dim1 == PS_DIMEN_IMAGE) {
        if (((psImage* ) in1)->numCols == 0 || ((psImage* ) in1)->numRows == 0) {
            psLogMsg(__func__, PS_LOG_WARN, ": Line %d - 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, ": Line %d - Vector contains zero elements");
        }
    } else if (dim2 == PS_DIMEN_IMAGE) {
        if (((psImage* ) in2)->numCols == 0 || ((psImage* ) in2)->numRows == 0) {
            psLogMsg(__func__, PS_LOG_WARN, ": Line %d - 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(__func__, "Couldn't create a proper output psVector.");
                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(__func__, "Couldn't create a proper output psImage.");
                return NULL;
            }
            BINARY_OP(SCALAR, IMAGE, out, psType1, op, psType2);        // scalar op image
        } else {
            psError(__func__, ": Line %d - Invalid dimensionality for in2 arg: %d", __LINE__, dim2);
        }
    } 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(__func__, "Couldn't create a proper output psVector.");
                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(__func__, "Couldn't create a proper output psVector.");
                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(__func__, "Couldn't create a proper output psImage.");
                return NULL;
            }
            BINARY_OP(VECTOR, IMAGE, out, psType1, op, psType2);        // vector op image
        } else {
            psError(__func__, ": Line %d - Invalid dimensionality for in2 arg: %d", __LINE__, dim2);
        }
    } else if (dim1 == PS_DIMEN_IMAGE) {
        out = psImageRecycle(out, ((psImage*)in1)->numCols, ((psImage*)in1)->numRows, elType1);
        if (out == NULL) {
            psError(__func__, "Couldn't create a proper output psImage.");
            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 {
            if (out != in1 && out != in2) {
                psFree(out);
            }
            psError(__func__, ": Line %d - Invalid dimensionality for in2 arg: %d", __LINE__, dim2);
            return NULL;
        }
    } else {
        if (out != in1 && out != in2) {
            psFree(out);
        }
        psError(__func__, ": Line %d - Invalid dimensionality for in1 arg: %d", __LINE__, dim1);
        return NULL;
    }

    // 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)                                                                               \
{                                                                                                            \
    int i = 0;                                                                                               \
    int nIn = 0;                                                                                             \
    int nOut = 0;                                                                                            \
    ps##TYPE *o = NULL;                                                                                      \
    ps##TYPE *i1 = NULL;                                                                                     \
    nIn = ((psVector* )IN)->n;                                                                               \
    nOut = ((psVector* )OUT)->n;                                                                             \
    if(nIn != nOut) {                                                                                        \
        psError(__func__, ": Inconsistent element count: %d vs %d", 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)                                                                                \
{                                                                                                            \
    int i = 0;                                                                                               \
    int j = 0;                                                                                               \
    int numRowsIn = 0;                                                                                       \
    int numColsIn = 0;                                                                                       \
    int numRowsOut = 0;                                                                                      \
    int 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(__func__, ": Inconsistent element count: numRows: %d vs %d numCols: %d vs %d", numRowsIn,    \
                numRowsOut, numColsIn, numColsOut);                                                          \
        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:                                                                                                     \
    psError(__func__, ": Invalid PS_TYPE: %d", IN->type);                                                    \
    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(__func__, ": Invalid operation: %s", OP);                                                        \
}

void *psUnaryOp(void *out, void *in, char *op)
{
    psDimen dimIn = 0;
    psElemType elTypeIn = 0;
    psType* psTypeIn = NULL;

    psTypeIn = (psType* ) in;
    if (psTypeIn == NULL) {
        psError(__func__, ": Line %d - Null in argument", __LINE__);
        if (out != in) {
            psFree(out);
        }
        return NULL;
    }

    if (op == NULL) {
        psError(__func__, ": Line %d - Null op argument", __LINE__);
        if (out != in) {
            psFree(out);
        }
        return NULL;
    }

    dimIn = psTypeIn->dimen;
    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) {
            if (out != in) {
                psFree(out);
            }
            psError(__func__, ": Line %d - Vector contains zero elements");
            return NULL;
        }

        out = psVectorRecycle(out,
                              ((psVector*)in)->n,
                              elTypeIn);
        if (out == NULL) {
            psError(__func__, "Couldn't create a proper output psVector.");
            return NULL;
        }

        UNARY_OP(VECTOR, out, psTypeIn, op);    // vector
        break;
    case PS_DIMEN_IMAGE:
        if (((psImage* ) in)->numCols == 0 || ((psImage* ) in)->numRows == 0) {
            if (out != in) {
                psFree(out);
            }
            psError(__func__, ": Line %d - Image contains zero length row or cols");
            return NULL;
        }

        out = psImageRecycle(out,
                             ((psImage*)in)->numCols,
                             ((psImage*)in)->numRows,
                             elTypeIn);
        if (out == NULL) {
            psError(__func__, "Couldn't create a proper output psImage.");
            return NULL;
        }

        UNARY_OP(IMAGE, out, psTypeIn, op);     // image
        break;
    default:
        if (out != in) {
            psFree(out);
        }
        psError(__func__, ": Line %d - Invalid dimensionality for in arg: %d", __LINE__, dimIn);
        return NULL;
    }

    // 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;
}
