/*============================================================================

  WCSLIB 5.1 - an implementation of the FITS WCS standard.
  Copyright (C) 1995-2015, Mark Calabretta

  This file is part of WCSLIB.

  WCSLIB is free software: you can redistribute it and/or modify it under the
  terms of the GNU Lesser General Public License as published by the Free
  Software Foundation, either version 3 of the License, or (at your option)
  any later version.

  WCSLIB 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 Lesser General Public License for
  more details.

  You should have received a copy of the GNU Lesser General Public License
  along with WCSLIB.  If not, see http://www.gnu.org/licenses.

  Direct correspondence concerning WCSLIB to mark@calabretta.id.au

  Author: Mark Calabretta, Australia Telescope National Facility, CSIRO.
  http://www.atnf.csiro.au/people/Mark.Calabretta
  $Id: dis.c,v 5.1 2015/04/07 10:56:36 mcalabre Exp $
*===========================================================================*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "wcserr.h"
#include "wcsprintf.h"
#include "wcsutil.h"
#include "dis.h"

const int DISSET = 137;

/* Map status return value to message. */
const char *dis_errmsg[] = {
  "Success",
  "Null disprm pointer passed",
  "Memory allocation failed",
  "Failed to initialize distortion functions",
  "Distort error",
  "De-distort error"};

/* Convenience macro for invoking wcserr_set(). */
#define DIS_ERRMSG(status) WCSERR_SET(status), dis_errmsg[status]

/*--------------------------------------------------------------------------*/

int disini(int alloc, int naxis, struct disprm *dis)

{
  static const char *function = "disini";

  int j;
  struct wcserr **err;

  if (dis == 0x0) return DISERR_NULL_POINTER;

  /* Initialize error message handling. */
  err = &(dis->err);
  if (dis->flag != -1) {
    if (dis->err) free(dis->err);
  }
  dis->err = 0x0;


  /* Initialize memory management. */
  if (dis->flag == -1 || dis->m_flag != DISSET) {
    if (dis->flag == -1) {
      dis->disp2x  = 0x0;
      dis->disx2p  = 0x0;
      dis->tmpmem  = 0x0;
      dis->iwrk    = 0x0;
      dis->dwrk    = 0x0;
    }

    dis->m_flag    = 0;
    dis->m_naxis   = 0;
    dis->m_dtype   = 0x0;
    dis->m_axmap   = 0x0;
    dis->m_offset  = 0x0;
    dis->m_scale   = 0x0;
    dis->m_nparm   = 0x0;
    dis->m_parms   = 0x0;
    dis->m_maxdis  = 0x0;
    dis->m_alloc   = 0;
    dis->m_padding = 0;
  }

  if (naxis < 0) {
    return wcserr_set(WCSERR_SET(DISERR_MEMORY),
      "naxis must not be negative (got %d)", naxis);
  }


  /* Create work arrays. */
  if (dis->disp2x) free(dis->disp2x);
  if ((dis->disp2x = calloc(naxis, sizeof(int (*)(DISP2X_ARGS)))) == 0x0) {
    disfree(dis);
    return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
  }

  if (dis->disx2p) free(dis->disx2p);
  if ((dis->disx2p = calloc(naxis, sizeof(int (*)(DISX2P_ARGS)))) == 0x0) {
    disfree(dis);
    return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
  }

  if (dis->tmpmem) free(dis->tmpmem);
  if ((dis->tmpmem = calloc(5*naxis, sizeof(double))) == 0x0) {
    disfree(dis);
    return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
  }

  if (dis->iwrk) free(dis->iwrk);
  if (dis->dwrk) free(dis->dwrk);


  /* Allocate memory for arrays if required. */
  if (alloc ||
      dis->dtype  == 0x0 ||
      dis->axmap  == 0x0 ||
      dis->offset == 0x0 ||
      dis->scale  == 0x0 ||
      dis->nparm  == 0x0 ||
      dis->parms  == 0x0 ||
      dis->maxdis == 0x0) {

    /* Was sufficient allocated previously? */
    if (dis->m_flag == DISSET && dis->m_naxis < naxis) {
      /* No, free it. */
      disfree(dis);
    }

    if (alloc || dis->dtype == 0x0) {
      if (dis->m_dtype) {
        /* In case the caller fiddled with it. */
        dis->dtype = dis->m_dtype;

      } else {
        if ((dis->dtype = calloc(naxis, sizeof(char [16]))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_dtype = dis->dtype;
      }
    }

    if (alloc || dis->axmap == 0x0) {
      if (dis->m_axmap) {
        /* In case the caller fiddled with it. */
        dis->axmap = dis->m_axmap;

      } else {
        if ((dis->axmap = calloc(naxis, sizeof(int *))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        if ((dis->axmap[0] = calloc(naxis*naxis, sizeof(int))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        for (j = 1; j < naxis; j++) {
          dis->axmap[j] = dis->axmap[j-1] + naxis;
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_axmap = dis->axmap;
      }
    }

    if (alloc || dis->offset == 0x0) {
      if (dis->m_offset) {
        /* In case the caller fiddled with it. */
        dis->offset = dis->m_offset;

      } else {
        if ((dis->offset = calloc(naxis, sizeof(double *))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        if ((dis->offset[0] = calloc(naxis*naxis, sizeof(double))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        for (j = 1; j < naxis; j++) {
          dis->offset[j] = dis->offset[j-1] + naxis;
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_offset = dis->offset;
      }
    }

    if (alloc || dis->scale == 0x0) {
      if (dis->m_scale) {
        /* In case the caller fiddled with it. */
        dis->scale = dis->m_scale;

      } else {
        if ((dis->scale = calloc(naxis, sizeof(double *))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        if ((dis->scale[0] = calloc(naxis*naxis, sizeof(double))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        for (j = 1; j < naxis; j++) {
          dis->scale[j] = dis->scale[j-1] + naxis;
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_scale = dis->scale;
      }
    }

    if (alloc || dis->nparm == 0x0) {
      if (dis->m_nparm) {
        /* In case the caller fiddled with it. */
        dis->nparm = dis->m_nparm;

      } else {
        if ((dis->nparm = calloc(naxis, sizeof(int))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_nparm = dis->nparm;
      }
    }

    if (alloc || dis->parms == 0x0) {
      if (dis->m_parms) {
        /* In case the caller fiddled with it. */
        dis->parms = dis->m_parms;

      } else {
        if ((dis->parms = calloc(naxis, sizeof(double *))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_parms = dis->parms;
      }
    }

    if (alloc || dis->maxdis == 0x0) {
      if (dis->m_maxdis) {
        /* In case the caller fiddled with it. */
        dis->maxdis = dis->m_maxdis;

      } else {
        if ((dis->maxdis = calloc(naxis, sizeof(double))) == 0x0) {
          disfree(dis);
          return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
        }

        dis->m_flag  = DISSET;
        dis->m_naxis = naxis;
        dis->m_maxdis = dis->maxdis;
      }
    }
  }

  dis->m_alloc = 0;


  /* Set defaults. */
  dis->flag  = 0;
  dis->naxis = naxis;

  memset(dis->dtype,     0, naxis*sizeof(char [16]));
  memset(dis->axmap[0],  0, naxis*naxis*sizeof(int));
  memset(dis->offset[0], 0, naxis*naxis*sizeof(double));

  for (j = 0; j < naxis*naxis; j++) {
    dis->scale[0][j] = 1.0;
  }

  memset(dis->nparm,     0, naxis*sizeof(int));
  memset(dis->parms,     0, naxis*sizeof(double *));
  memset(dis->maxdis,    0, naxis*sizeof(double));
  dis->totdis = 0.0;

  dis->ndis = 0;
  dis->padding = 0;

  return 0;
}

/*--------------------------------------------------------------------------*/

int disparms(struct disprm *dis)

{
  static const char *function = "disparms";

  int j;
  struct wcserr **err;

  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);


  /* Free memory previously allocated. */
  if (dis->m_alloc) {
    for (j = 0; j < dis->naxis; j++) {
      if (dis->parms[j]) {
        free(dis->parms[j]);
        dis->parms[j] = 0x0;
      }
    }
  }

  /* Allocate memory for distortion parameter arrays. */
  for (j = 0; j < dis->naxis; j++) {
    if (dis->nparm[j]) {
      if ((dis->parms[j] = calloc(dis->nparm[j], sizeof(double))) == 0x0) {
        return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
      }

      dis->m_flag  = DISSET;
      dis->m_alloc = 1;
    }
  }

  return 0;
}


/*--------------------------------------------------------------------------*/

int discpy(int alloc, const struct disprm *dissrc, struct disprm *disdst)

{
  static const char *function = "discpy";

  int j, naxis, status;
  struct wcserr **err;

  if (dissrc == 0x0) return DISERR_NULL_POINTER;
  if (disdst == 0x0) return DISERR_NULL_POINTER;
  err = &(disdst->err);

  naxis = dissrc->naxis;
  if (naxis < 1) {
    return wcserr_set(WCSERR_SET(DISERR_MEMORY),
      "naxis must be positive (got %d)", naxis);
  }

  if ((status = disini(alloc, naxis, disdst))) {
    return status;
  }

  memcpy(disdst->dtype, dissrc->dtype, naxis*sizeof(char [16]));
  for (j = 0; j < naxis; j++) {
    memcpy(disdst->axmap[j],  dissrc->axmap[j],  naxis*sizeof(int));
    memcpy(disdst->offset[j], dissrc->offset[j], naxis*sizeof(double));
    memcpy(disdst->scale[j],  dissrc->scale[j],  naxis*sizeof(double));
  }
  memcpy(disdst->nparm, dissrc->nparm, naxis*sizeof(int));

  if (alloc) {
    disparms(disdst);
  }

  for (j = 0; j < naxis; j++) {
    if (dissrc->nparm[j]) {
      memcpy(disdst->parms[j], dissrc->parms[j],
             dissrc->nparm[j]*sizeof(double));
    }
  }

  memcpy(disdst->maxdis, dissrc->maxdis, naxis*sizeof(double));
  disdst->totdis = dissrc->totdis;

  return 0;
}

/*--------------------------------------------------------------------------*/

int disfree(struct disprm *dis)

{
  int j;

  if (dis == 0x0) return DISERR_NULL_POINTER;

  if (dis->flag != -1) {
    /* Free memory allocated by disini(). */
    if (dis->m_flag == DISSET) {
      /* Recall that these were allocated in bulk. */
      if (dis->m_axmap)  free(dis->axmap[0]);
      if (dis->m_offset) free(dis->offset[0]);
      if (dis->m_scale)  free(dis->scale[0]);

      if (dis->m_alloc) {
        for (j = 0; j < dis->naxis; j++) {
          if (dis->parms[j]) {
            free(dis->parms[j]);
            dis->parms[j] = 0x0;
          }
        }
      }

      if (dis->dtype  == dis->m_dtype)  dis->dtype  = 0x0;
      if (dis->axmap  == dis->m_axmap)  dis->axmap  = 0x0;
      if (dis->offset == dis->m_offset) dis->offset = 0x0;
      if (dis->scale  == dis->m_scale)  dis->scale  = 0x0;
      if (dis->nparm  == dis->m_nparm)  dis->nparm  = 0x0;
      if (dis->parms  == dis->m_parms)  dis->parms  = 0x0;
      if (dis->maxdis == dis->m_maxdis) dis->maxdis = 0x0;

      if (dis->disp2x) free(dis->disp2x);
      if (dis->disx2p) free(dis->disx2p);
      if (dis->tmpmem) free(dis->tmpmem);
      if (dis->iwrk)   free(dis->iwrk);
      if (dis->dwrk)   free(dis->dwrk);

      if (dis->m_dtype)  free(dis->m_dtype);
      if (dis->m_axmap)  free(dis->m_axmap);
      if (dis->m_offset) free(dis->m_offset);
      if (dis->m_scale)  free(dis->m_scale);
      if (dis->m_nparm)  free(dis->m_nparm);
      if (dis->m_parms)  free(dis->m_parms);
      if (dis->m_maxdis) free(dis->m_maxdis);
    }
  }

  dis->disp2x  = 0x0;
  dis->disx2p  = 0x0;
  dis->tmpmem  = 0x0;
  dis->iwrk    = 0x0;
  dis->dwrk    = 0x0;

  dis->m_flag   = 0;
  dis->m_naxis  = 0;
  dis->m_dtype  = 0x0;
  dis->m_axmap  = 0x0;
  dis->m_offset = 0x0;
  dis->m_scale  = 0x0;
  dis->m_nparm  = 0x0;
  dis->m_parms  = 0x0;
  dis->m_maxdis = 0x0;
  dis->m_alloc  = 0;

  dis->flag = 0;

  if (dis->err) {
    free(dis->err);
    dis->err = 0x0;
  }

  return 0;
}

/*--------------------------------------------------------------------------*/

int disprt(const struct disprm *dis)

{
  char hext[32];
  int i, j, k;

  if (dis == 0x0) return DISERR_NULL_POINTER;

  if (dis->flag != DISSET) {
    wcsprintf("The disprm struct is UNINITIALIZED.\n");
    return 0;
  }
  wcsprintf("       flag: %d\n", dis->flag);

  /* Parameters supplied. */
  wcsprintf("      naxis: %d\n", dis->naxis);

  WCSPRINTF_PTR("      dtype: ", dis->dtype, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("             \"%s\"\n", dis->dtype[j]);
  }

  WCSPRINTF_PTR("      axmap: ", dis->axmap, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf(" axmap[%d][]:", j);
    for (i = 0; i < dis->naxis; i++) {
      wcsprintf("%6d", dis->axmap[j][i]);
    }
    wcsprintf("\n");
  }

  WCSPRINTF_PTR("     offset: ", dis->offset, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("offset[%d][]:", j);
    for (i = 0; i < dis->naxis; i++) {
      wcsprintf("  %#- 11.5g", dis->offset[j][i]);
    }
    wcsprintf("\n");
  }

  WCSPRINTF_PTR("      scale: ", dis->scale, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf(" scale[%d][]:", j);
    for (i = 0; i < dis->naxis; i++) {
      wcsprintf("  %#- 11.5g", dis->scale[j][i]);
    }
    wcsprintf("\n");
  }

  WCSPRINTF_PTR("      nparm: ", dis->nparm, "\n");
  wcsprintf("            ");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("%6d", dis->nparm[j]);
  }
  wcsprintf("\n");

  WCSPRINTF_PTR("      parms: ", dis->parms, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf(" parms[%d][]:", j);
    for (k = 0; k < dis->nparm[j]; k++) {
      if (k && k%5 == 0) {
        wcsprintf("\n            ");
      }
      wcsprintf("  %#- 11.5g", dis->parms[j][k]);
    }
    wcsprintf("\n");
  }

  WCSPRINTF_PTR("     maxdis: ", dis->maxdis, "\n");
  wcsprintf("            ");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("  %#- 11.5g", dis->maxdis[j]);
  }
  wcsprintf("\n");

  wcsprintf("     totdis:  %#- 11.5g\n", dis->totdis);

  /* Derived values. */
  wcsprintf("       ndis: %d\n", dis->ndis);

  /* Error handling. */
  WCSPRINTF_PTR("        err: ", dis->err, "\n");
  if (dis->err) {
    wcserr_prt(dis->err, "             ");
  }

  /* Work arrays. */
  WCSPRINTF_PTR("     disp2x: ", dis->disp2x, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("  disp2x[%d]: %s", j,
      wcsutil_fptr2str((int (*)(void))dis->disp2x[j], hext));
    if (dis->disp2x[j] == tpv1) {
      wcsprintf("  (= tpv1)\n");
    } else if (dis->disp2x[j] == tpv2) {
      wcsprintf("  (= tpv2)\n");
    } else if (dis->disp2x[j] == tpv3) {
      wcsprintf("  (= tpv3)\n");
    } else if (dis->disp2x[j] == tpv4) {
      wcsprintf("  (= tpv4)\n");
    } else if (dis->disp2x[j] == tpv5) {
      wcsprintf("  (= tpv5)\n");
    } else if (dis->disp2x[j] == tpv6) {
      wcsprintf("  (= tpv6)\n");
    } else if (dis->disp2x[j] == tpv7) {
      wcsprintf("  (= tpv7)\n");
    } else {
      wcsprintf("\n");
    }
  }
  WCSPRINTF_PTR("     disx2p: ", dis->disx2p, "\n");
  for (j = 0; j < dis->naxis; j++) {
    wcsprintf("  disx2p[%d]: %s\n", j,
      wcsutil_fptr2str((int (*)(void))dis->disx2p[j], hext));
  }
  WCSPRINTF_PTR("     tmpmem: ", dis->tmpmem, "\n");
  WCSPRINTF_PTR("       iwrk: ", dis->iwrk, "\n");
  WCSPRINTF_PTR("       dwrk: ", dis->dwrk, "\n");

  /* Memory management. */
  wcsprintf("     m_flag: %d\n", dis->m_flag);
  wcsprintf("    m_naxis: %d\n", dis->m_naxis);
  WCSPRINTF_PTR("    m_dtype: ", dis->m_dtype, "");
  if (dis->m_dtype  == dis->dtype)  wcsprintf("  (= dtype)");
  wcsprintf("\n");
  WCSPRINTF_PTR("    m_axmap: ", dis->m_axmap, "");
  if (dis->m_axmap  == dis->axmap)  wcsprintf("  (= axmap)");
  wcsprintf("\n");
  WCSPRINTF_PTR("   m_offset: ", dis->m_offset, "");
  if (dis->m_offset == dis->offset) wcsprintf("  (= offset)");
  wcsprintf("\n");
  WCSPRINTF_PTR("    m_scale: ", dis->m_scale, "");
  if (dis->m_scale  == dis->scale)  wcsprintf("  (= scale)");
  wcsprintf("\n");
  WCSPRINTF_PTR("    m_nparm: ", dis->m_nparm, "");
  if (dis->m_nparm  == dis->nparm)  wcsprintf("  (= nparm)");
  wcsprintf("\n");
  WCSPRINTF_PTR("    m_parms: ", dis->m_parms, "");
  if (dis->m_parms  == dis->parms)  wcsprintf("  (= parms)");
  wcsprintf("\n");
  WCSPRINTF_PTR("   m_maxdis: ", dis->m_maxdis, "");
  if (dis->m_maxdis == dis->maxdis) wcsprintf("  (= maxdis)");
  wcsprintf("\n");

  return 0;
}

/*--------------------------------------------------------------------------*/

int disset(struct disprm *dis)

{
  static const char *function = "disset";

  int j, naxis, ndis, status;
  struct wcserr **err;

  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);

  naxis = dis->naxis;

  /* Identify the distortion functions. */
  ndis = 0;
  for (j = 0; j < naxis; j++) {
    if (strlen(dis->dtype[j]) == 0) {
      /* No distortion on this axis. */
      continue;

    } else if (strcmp(dis->dtype[j], "TPV") == 0) {
      /* The TPV "projection" is a special case. */
      if ((status = tpvset(j, dis))) {
        return wcserr_set(DIS_ERRMSG(status));
      }

    } else {
      return wcserr_set(WCSERR_SET(DISERR_BAD_PARAM),
        "Unrecognized/unimplemented distortion function, %s", dis->dtype[j]);
    }

    ndis++;
  }

  dis->ndis = ndis;
  dis->flag = DISSET;

  return 0;
}

/*--------------------------------------------------------------------------*/

int disp2x(
  struct disprm *dis,
  const double rawcrd[],
  double discrd[])

{
  static const char *function = "disp2x";

  int axisi, i, j, naxis, ncrd, status;
  double dtmp, *tmpcrd;
  struct wcserr **err;


  /* Initialize. */
  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);

  if (dis->flag != DISSET) {
    if ((status = disset(dis))) return status;
  }

  naxis = dis->naxis;


  /* Invoke the distortion functions for each axis. */
  tmpcrd = dis->tmpmem;
  for (j = 0; j < naxis; j++) {
    if (dis->disp2x[j]) {
      ncrd = 0;
      for (i = 0; i < naxis; i++) {
        if ((axisi = dis->axmap[j][i])) {
          /* N.B. the axis numbers in the map are 1-relative. */
          tmpcrd[i] = (rawcrd[axisi-1] - dis->offset[j][i])*dis->scale[j][i];
          ncrd++;
        } else {
          tmpcrd[i] = 0.0;
        }
      }

      if ((status = (dis->disp2x[j])(dis->nparm[j], dis->parms[j], ncrd,
                                     tmpcrd, &dtmp))) {
        return wcserr_set(DIS_ERRMSG(DISERR_DISTORT));
      }

      discrd[j] = dtmp;

    } else {
      discrd[j] = rawcrd[j];
    }
  }

  return 0;
}

/*--------------------------------------------------------------------------*/

int disx2p(
  struct disprm *dis,
  const double discrd[],
  double rawcrd[])

{
  static const char *function = "disx2p";

  const int ITERMAX = 30;
  const double TOL = 1.0e-13;

  int convergence, iter, j, naxis, status;
  double dd, *dcrd0, *dcrd1, *delta, residual, *rcrd1;
  struct wcserr **err;


  /* Initialize. */
  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);

  naxis = dis->naxis;

  /* Carve up working memory, noting that disp2x() gets to it first. */
  dcrd0 = dis->tmpmem + naxis;
  dcrd1 = dcrd0 + naxis;
  rcrd1 = dcrd1 + naxis;
  delta = rcrd1 + naxis;


  /* Zeroth approximation.  The assumption here and below is that the     */
  /* distortion is small so that, to first order in the neighbourhood of  */
  /* the solution, discrd[j] ~= a + b*rawcrd[j], i.e. independent of      */
  /* rawcrd[i], where i != j.  This is effectively equivalent to assuming */
  /* that the distortion functions are separable to first order.          */
  /* Furthermore, a is assumed to be small, and b close to unity.         */
  memcpy(rawcrd, discrd, naxis*sizeof(double));

  /* Iteratively invert the (well-behaved!) distortion function. */
  for (iter = 0; iter < ITERMAX; iter++) {
    if ((status = disp2x(dis, rawcrd, dcrd0))) {
      return wcserr_set(DIS_ERRMSG(status));
    }

    /* Check for convergence. */
    convergence = 1;
    for (j = 0; j < naxis; j++) {
      delta[j] = discrd[j] - dcrd0[j];

      if (fabs(discrd[j]) < 1.0) {
        dd = delta[j];
      } else {
        /* TOL may be below the precision achievable from floating point */
        /* subtraction, so switch to a fractional tolerance.             */
        dd = delta[j] / discrd[j];
      }

      if (TOL < fabs(dd)) {
        /* No convergence yet on this axis. */
        convergence = 0;
      }
    }

    if (convergence) break;

    /* Determine a suitable test point for computing the gradient. */
    for (j = 0; j < naxis; j++) {
      /* Constrain the displacement. */
      delta[j] /= 2.0;
      if (fabs(delta[j]) < 1.0e-6) {
        if (delta[j] < 0.0) {
          delta[j] = -1.0e-6;
        } else {
          delta[j] =  1.0e-6;
        }
      } else if (1.0 < fabs(delta[j])) {
        if (delta[j] < 0.0) {
          delta[j] = -1.0;
        } else {
          delta[j] =  1.0;
        }
      }
    }

    if (iter < ITERMAX/2) {
      /* With the assumption of small distortions (as above), the gradient */
      /* of discrd[j] should be dominated by the partial derivative with   */
      /* respect  to rawcrd[j], and we can neglect partials with respect   */
      /* to rawcrd[i], where i != j.  Thus only one test point is needed,  */
      /* not one for each axis.                                            */
      for (j = 0; j < naxis; j++) {
        rcrd1[j] = rawcrd[j] + delta[j];
      }

      /* Compute discrd[] at the test point. */
      if ((status = disp2x(dis, rcrd1, dcrd1))) {
        return wcserr_set(DIS_ERRMSG(status));
      }

      /* Compute the next approximation. */
      for (j = 0; j < naxis; j++) {
        rawcrd[j] += (discrd[j] - dcrd0[j]) *
                        (delta[j]/(dcrd1[j] - dcrd0[j]));
      }

    } else {
      /* Convergence should not take more than seven or so iterations.  As */
      /* it is slow, try computing the gradient in full.                   */
      memcpy(rcrd1, rawcrd, naxis*sizeof(double));

      for (j = 0; j < naxis; j++) {
        rcrd1[j] += delta[j];

        /* Compute discrd[] at the test point. */
        if ((status = disp2x(dis, rcrd1, dcrd1))) {
          return wcserr_set(DIS_ERRMSG(status));
        }

        /* Compute the next approximation. */
        rawcrd[j] += (discrd[j] - dcrd0[j]) *
                       (delta[j]/(dcrd1[j] - dcrd0[j]));

        rcrd1[j] -= delta[j];
      }
    }
  }


  if (!convergence) {
    residual = 0.0;
    for (j = 0; j < naxis; j++) {
      dd = discrd[j] - dcrd0[j] ;
      residual += dd*dd;
    }
    residual = sqrt(residual);

    return wcserr_set(WCSERR_SET(DISERR_DEDISTORT),
      "Convergence not achieved after %d iterations, residual %#7.2g", iter,
        residual);
  }


  return 0;
}

/*--------------------------------------------------------------------------*/

int diswarp(
  struct disprm *dis,
  const double pixblc[],
  const double pixtrc[],
  const double pixsamp[],
  int    *nsamp,
  double maxdis[],
  double *maxtot,
  double avgdis[],
  double *avgtot,
  double rmsdis[],
  double *rmstot)

{
  static const char *function = "diswarp";

  int carry, j, naxis, status = 0;
  double dpix, dpx2, dssq, *pix0, *pix1, *pixend, *pixinc, pixspan, *ssqdis,
         ssqtot, *sumdis, sumtot, totdis;
  struct wcserr **err;


  /* Initialize. */
  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);

  naxis = dis->naxis;

  if (nsamp) *nsamp = 0;
  for (j = 0; j < naxis; j++) {
    if (maxdis) maxdis[j] = 0.0;
    if (avgdis) avgdis[j] = 0.0;
    if (rmsdis) rmsdis[j] = 0.0;
  }
  if (maxtot) *maxtot = 0.0;
  if (avgtot) *avgtot = 0.0;
  if (rmstot) *rmstot = 0.0;

  /* Quick return if no distortions. */
  if (dis->ndis == 0) return 0;

  /* Carve up working memory, noting that disp2x() gets to it first. */
  pixinc = dis->tmpmem + naxis;
  pixend = pixinc + naxis;
  sumdis = pixend + naxis;
  ssqdis = sumdis + naxis;

  /* Work out increments on each axis. */
  for (j = 0; j < naxis; j++) {
    pixspan = pixtrc[j] - (pixblc ? pixblc[j] : 1.0);

    if (pixsamp == 0x0) {
      pixinc[j] = 1.0;
    } else if (pixsamp[j] == 0.0) {
      pixinc[j] = 1.0;
    } else if (pixsamp[j] > 0.0) {
      pixinc[j] = pixsamp[j];
    } else if (pixsamp[j] > -1.5) {
      pixinc[j] = 2.0*pixspan;
    } else {
      pixinc[j] = pixspan / ((int)(-pixsamp[j] - 0.5));
    }
  }

  /* Get some more memory for coordinate vectors. */
  if ((pix0 = calloc(2*naxis, sizeof(double))) == 0x0) {
    return wcserr_set(DIS_ERRMSG(DISERR_MEMORY));
  }

  pix1 = pix0 + naxis;


  /* Set up the array of pixel coordinates. */
  for (j = 0; j < naxis; j++) {
    pix0[j] = pixblc ? pixblc[j] : 1.0;
    pixend[j] = pixtrc[j] + 0.5*pixinc[j];
  }

  /* Initialize accumulators. */
  for (j = 0; j < naxis; j++) {
    sumdis[j] = 0.0;
    ssqdis[j] = 0.0;
  }
  sumtot = 0.0;
  ssqtot = 0.0;


  /* Loop over N dimensions. */
  carry = 0;
  while (carry == 0) {
    if ((status = disp2x(dis, pix0, pix1))) {
      wcserr_set(DIS_ERRMSG(status));
      goto cleanup;
    }

    /* Accumulate statistics. */
    (*nsamp)++;

    dssq = 0.0;
    for (j = 0; j < naxis; j++) {
      dpix = pix1[j] - pix0[j];
      dpx2 = dpix*dpix;

      sumdis[j] += dpix;
      ssqdis[j] += dpx2;

      if (maxdis && (dpix = fabs(dpix)) > maxdis[j]) maxdis[j] = dpix;

      dssq += dpx2;
    }

    sumtot += totdis;
    ssqtot += totdis*totdis;

    totdis = sqrt(dssq);
    if (maxtot && *maxtot < totdis) *maxtot = totdis;

    /* Next pixel. */
    for (j = 0; j < naxis; j++) {
      pix0[j] += pixinc[j];
      if (pix0[j] < pixend[j]) {
        carry = 0;
        break;
      }

      pix0[j] = pixblc ? pixblc[j] : 1.0;
      carry = 1;
    }
  }


  /* Compute the means and RMSs. */
  for (j = 0; j < naxis; j++) {
    ssqdis[j] /= *nsamp;
    sumdis[j] /= *nsamp;
    if (avgdis) avgdis[j] = sumdis[j];
    if (rmsdis) rmsdis[j] = sqrt(ssqdis[j] - sumdis[j]*sumdis[j]);
  }

  ssqtot /= *nsamp;
  sumtot /= *nsamp;
  if (avgtot) *avgtot = sumtot;
  if (rmstot) *rmstot = sqrt(ssqtot - sumtot*sumtot);


cleanup:
  free(pix0);

  return status;
}

/*--------------------------------------------------------------------------*/

int tpvset(int j, struct disprm *dis)

{
  static const char *function = "tpvset";

  int i, nparm;
  struct wcserr **err;

  if (dis == 0x0) return DISERR_NULL_POINTER;
  err = &(dis->err);


  /* TPV "projection". */
  if (dis->axmap[j][0] == 0) {
    return wcserr_set(WCSERR_SET(DISERR_BAD_PARAM),
      "Invalid axis 1 mapping for TPV on axis %d", j);
  }

  if (dis->axmap[j][1] == 0 || dis->axmap[j][1] == dis->axmap[j][0]) {
    return wcserr_set(WCSERR_SET(DISERR_BAD_PARAM),
      "Invalid axis 2 mapping for TPV on axis %d", j);
  }

  for (i = 2; i < dis->naxis; i++) {
    if (dis->axmap[j][i]) {
      return wcserr_set(WCSERR_SET(DISERR_BAD_PARAM),
        "Invalid axis map for TPV on axis %d, more than two entries", j);
    }
  }

  /* Determine the degree of the polynomial. */
  nparm = dis->nparm[j];
  if (nparm == 4) {
    /* First degree. */
    dis->disp2x[j] = tpv1;
  } else if (nparm == 7) {
    /* Second degree. */
    dis->disp2x[j] = tpv2;
  } else if (nparm == 12) {
    /* Third degree. */
    dis->disp2x[j] = tpv3;
  } else if (nparm == 17) {
    /* Fourth degree. */
    dis->disp2x[j] = tpv4;
  } else if (nparm == 24) {
    /* Fifth degree. */
    dis->disp2x[j] = tpv5;
  } else if (nparm == 31) {
    /* Sixth degree. */
    dis->disp2x[j] = tpv6;
  } else if (nparm == 40) {
    /* Seventh degree. */
    dis->disp2x[j] = tpv7;
  } else {
    return wcserr_set(WCSERR_SET(DISERR_BAD_PARAM),
      "Invalid number of parameters (%d) for TPV on axis %d", nparm, j);
  }

  /* No specialist deprojections. */
  dis->disx2p[j] = 0x0;

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv1(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 4 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* First degree. */
  *discrd = p[0] + u*p[1] + v*p[2] + r*p[3];

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv2(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 7 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Second degree. */
  *discrd =
         p[0]  + r*(p[3])
               + v*(p[2]  + v*(p[6]))
    + u*(p[1]  + v*(p[5])
    + u*(p[4]));

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv3(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 12 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Third degree. */
  *discrd =
         p[0]  + r*(p[3]  +            s*(p[11]))
               + v*(p[2]  + v*(p[6]  + v*(p[10])))
    + u*(p[1]  + v*(p[5]  + v*(p[9]))
    + u*(p[4]  + v*(p[8])
    + u*(p[7])));

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv4(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 17 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Fourth degree. */
  *discrd =
         p[0]  + r*(p[3]  +            s*(p[11]))
               + v*(p[2]  + v*(p[6]  + v*(p[10] + v*(p[16]))))
    + u*(p[1]  + v*(p[5]  + v*(p[9]  + v*(p[15])))
    + u*(p[4]  + v*(p[8]  + v*(p[14]))
    + u*(p[7]  + v*(p[13])
    + u*(p[12]))));

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv5(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 24 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Fifth degree. */
  *discrd =
         p[0]  + r*(p[3]  +            s*(p[11] +            s*(p[23])))
               + v*(p[2]  + v*(p[6]  + v*(p[10] + v*(p[16] + v*(p[22])))))
    + u*(p[1]  + v*(p[5]  + v*(p[9]  + v*(p[15] + v*(p[21]))))
    + u*(p[4]  + v*(p[8]  + v*(p[14] + v*(p[20])))
    + u*(p[7]  + v*(p[13] + v*(p[19]))
    + u*(p[12] + v*(p[18])
    + u*(p[17])))));

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv6(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 31 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Sixth degree. */
  *discrd =
         p[0]  + r*(p[3]  +            s*(p[11] +            s*(p[23])))
               + v*(p[2]  + v*(p[6]  + v*(p[10] + v*(p[16] + v*(p[22] + v*(p[30]))))))
    + u*(p[1]  + v*(p[5]  + v*(p[9]  + v*(p[15] + v*(p[21] + v*(p[29])))))
    + u*(p[4]  + v*(p[8]  + v*(p[14] + v*(p[20] + v*(p[28]))))
    + u*(p[7]  + v*(p[13] + v*(p[19] + v*(p[27])))
    + u*(p[12] + v*(p[18] + v*(p[26]))
    + u*(p[17] + v*(p[25])
    + u*(p[24]))))));

  return 0;
}

/*--------------------------------------------------------------------------*/

int tpv7(
  int nparm,
  const double p[],
  int ncrd,
  const double rawcrd[],
  double *discrd)

{
  double r, s, u, v;

  if (nparm != 40 || ncrd != 2) {
    return 1;
  }

  u = rawcrd[0];
  v = rawcrd[1];
  s = u*u + v*v;
  r = sqrt(s);

  /* Seventh degree. */
  *discrd =
         p[0]  + r*(p[3]  +            s*(p[11] +            s*(p[23] +            s*(p[39]))))
               + v*(p[2]  + v*(p[6]  + v*(p[10] + v*(p[16] + v*(p[22] + v*(p[30] + v*(p[38])))))))
    + u*(p[1]  + v*(p[5]  + v*(p[9]  + v*(p[15] + v*(p[21] + v*(p[29] + v*(p[37]))))))
    + u*(p[4]  + v*(p[8]  + v*(p[14] + v*(p[20] + v*(p[28] + v*(p[36])))))
    + u*(p[7]  + v*(p[13] + v*(p[19] + v*(p[27] + v*(p[35]))))
    + u*(p[12] + v*(p[18] + v*(p[26] + v*(p[34])))
    + u*(p[17] + v*(p[25] + v*(p[33]))
    + u*(p[24] + v*(p[32])
    + u*(p[31])))))));

  return 0;
}
