// Copyright (C) 2006  Paul A. Price (price@ifa.hawaii.edu)
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <assert.h>
#include "pslib.h"
#include "pois.h"

void poisAddPenalty(psImage *matrix,    // The matrix
                    psVector *vector,   // The vector
                    float penalty,      // Penalty value
                    const psArray *kernels, // Kernel basis functions
                    const poisConfig *config // Configuration
    )
{
    // Check inputs
    assert(matrix);
    assert(vector);
    assert(kernels);
    assert(config);
    assert(matrix->numRows == matrix->numCols);
    assert(vector->n == matrix->numRows);
    assert(vector->n == kernels->n + 1);// Additional space for constant background
    assert(matrix->type.type == PS_TYPE_F64);
    assert(vector->type.type == PS_TYPE_F64);

    int numKernels = kernels->n;

    for (int j = 0; j < numKernels; j++) {
        poisKernelBasis *kernel = kernels->data[j];
        if (kernel->xOrder == 0 && kernel->yOrder == 0) {
            for (int k = j + 1; k < numKernels; k++) {
                poisKernelBasis *compare = kernels->data[k];
                if (compare->xOrder == 0 && compare->yOrder == 0 &&
                    (compare->u == kernel->u + 1 || compare->u == kernel->u - 1 ||
                     compare->v == kernel->v + 1 || compare->v == kernel->v - 1)) {
                    matrix->data.F64[j][k] -= penalty;
                    matrix->data.F64[k][j] -= penalty;
                }
            }
        }

        if (abs(kernel->u) == config->xKernel) {
            if (abs(kernel->v) == config->yKernel) {
                vector->data.F64[j] -= 3.0 * penalty;
            } else {
                vector->data.F64[j] -= 5.0 * penalty;
            }
        } else {
            if (abs(kernel->v) == config->yKernel) {
                vector->data.F64[j] -= 5.0 * penalty;
            } else {
                vector->data.F64[j] -= 8.0 * penalty;
            }
        }

    }


}
