/*
 * distort.c - distort an image to create a panorama effect.
 *
 * Version 1.0
 * Copyright (C) 1998 Gustav Taxen.
 * 
 * This is free software with ABSOLUTELY NO WARRANTY.
 *
 * 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, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <malloc.h>
#include <assert.h>

#include <image.h>
#include <distort.h>

#define  FRONT    0
#define  LEFT     1
#define  BACK     2
#define  RIGHT    3
#define  UP       4
#define  DOWN     5


void
sphCoord (point   cartesian,
	  point   spherical)
{
  cartesian[0] = spherical[0] * cos (spherical[1]) * cos (spherical[2]);
  cartesian[1] = spherical[0] * sin (spherical[1]) * cos (spherical[2]);
  cartesian[2] = spherical[0] * sin (spherical[2]);
}


void
sphCoordInv (point   spherical,
	     point   cartesian)
{
  spherical[0] = sqrt (cartesian[0] * cartesian[0]
		       + cartesian[1] * cartesian[1]
		       + cartesian[2] * cartesian[2]);
  spherical[1] = atan2 (cartesian[1], cartesian[0]);
  spherical[2] = atan2 (cartesian[2], sqrt (cartesian[0] * cartesian[0]
					    + cartesian[1] * cartesian[1]));
}


float
mapImgCoordToZeroOne (int    p,
		      int    dimension)
{
  /* 0 <= p <= dimension - 1; map to [0, 1] */

  assert (p >= 0);
  assert (p <= dimension - 1);

  return (float) p / (float) (dimension - 1);
}


int
mapZeroOneToImgCoord (float    p,
		      int      dimension)
{
  /* 0 <= p <= 1; map to [0, dimension) */

  assert (p >= 0.0);
  assert (p <= 1.0);

  /*  return rint (p * (float) (dimension - 1));*/
  return floor (p * (float) (dimension - 1));
}


float
mapImgCoordToMinusOneOne (int    p,
			  int    dimension)
{
  /* 0 <= p <= dimension - 1; map to [-1, 1] */

  assert (p >= 0);
  assert (p <= dimension - 1);

  return 2.0 * ((float) p / (float) (dimension - 1)) - 1.0;
}


int
mapMinusOneOneToImgCoord (float    p,
			  int      dimension)
{
  /* -1 <= p <= 1; map to [0, dimension) */

  assert (p >= -1.0);
  assert (p <= 1.0);
  
  /*  return rint (((p + 1.0) / 2.0) * (float) (dimension - 1));*/
  return floor (((p + 1.0) / 2.0) * (float) (dimension - 1));
}


int
identifyImageFromDirection (float    lambda,
			    float    phi)
{
  if (phi > M_PI / 4.0)
    {
      return UP;
    }

  if (phi < -M_PI / 4.0)
    {
      return DOWN;
    }

  if (lambda >= -M_PI / 4.0 &&
      lambda < M_PI / 4.0)
    {
      return FRONT;
    }

  if (lambda >= M_PI / 4.0 &&
      lambda < 3 * (M_PI / 4.0))
    {
      return LEFT;
    }

  if (lambda >= -3 * (M_PI / 4.0) &&
      lambda < -M_PI / 4.0)
    {
      return RIGHT;
    }

  assert (lambda >= 3 * (M_PI / 4.0) ||
	  lambda < -3 * (M_PI / 4.0));

  return BACK;
}


int
identifyImageFromLambda (float    lambda)
{
  if (lambda >= -M_PI / 4.0 &&
      lambda < M_PI / 4.0)
    {
      return FRONT;
    }

  if (lambda >= M_PI / 4.0 &&
      lambda < 3 * (M_PI / 4.0))
    {
      return LEFT;
    }

  if (lambda >= -3 * (M_PI / 4.0) &&
      lambda < -M_PI / 4.0)
    {
      return RIGHT;
    }

  assert (lambda >= 3 * (M_PI / 4.0) ||
	  lambda < -3 * (M_PI / 4.0));

  return BACK;
}


char genericPixel[3] = {'a', 'b', 'z'};
char whitePixel[3] = {'v', 'v', 'v'};


char *
mapSphericalToPixel (point      s,
		     image     *src)
{
  float    lambda;
  float    phi;
  float    xi;
  int      srcx, srcy;
  int      img;

  lambda = s[1];
  phi = s[2];
  xi = (M_PI / 2.0) - phi;

  img = identifyImageFromDirection (lambda, phi);

  switch (img)
    {
    case FRONT:
      lambda = -lambda / (M_PI / 4.0);
      phi = -phi / (M_PI / 4.0);
      srcx = mapMinusOneOneToImgCoord (lambda, src[FRONT].width);
      srcy = mapMinusOneOneToImgCoord (phi, src[FRONT].height);
      break;

    case LEFT:
      lambda = -(lambda - (M_PI / 2.0)) / (M_PI / 4.0);
      phi = -phi / (M_PI / 4.0);
      srcx = mapMinusOneOneToImgCoord (lambda, src[RIGHT].width);
      srcy = mapMinusOneOneToImgCoord (phi, src[RIGHT].height);
      break;

    case RIGHT:
      lambda = -(lambda + (M_PI / 2.0)) / (M_PI / 4.0);
      phi = -phi / (M_PI / 4.0);
      srcx = mapMinusOneOneToImgCoord (lambda, src[LEFT].width);
      srcy = mapMinusOneOneToImgCoord (phi, src[LEFT].height);
      break;

    case BACK:
      if (lambda > 0)
	{
	  lambda = -(lambda - M_PI) / (M_PI / 4.0);
	}
      else
	{
	  lambda = -(lambda + M_PI) / (M_PI / 4.0);
	}
      phi = -phi / (M_PI / 4.0);
      srcx = mapMinusOneOneToImgCoord (lambda, src[BACK].width);
      srcy = mapMinusOneOneToImgCoord (phi, src[BACK].height);
      break;

    case UP:
      switch (identifyImageFromLambda (lambda))
	{
	case RIGHT:
	  lambda = (lambda + (M_PI / 2.0)) / (M_PI / 4.0);
	  phi = 1 - ((phi - (M_PI / 4.0)) / (M_PI / 4.0));
	  srcx = mapMinusOneOneToImgCoord (phi, src[UP].width);
	  srcy = mapMinusOneOneToImgCoord (phi * lambda, src[UP].height);
	  break;

	case FRONT:
	  lambda = -lambda / (M_PI / 4.0);
	  phi = 1 - ((phi - (M_PI / 4.0)) / (M_PI / 4.0));
	  srcy = mapMinusOneOneToImgCoord (phi, src[UP].height);
	  srcx = mapMinusOneOneToImgCoord (phi * lambda, src[UP].width);
	  break;

	case LEFT:
	  lambda = (lambda - (M_PI / 2.0)) / (M_PI / 4.0);
	  phi = ((phi - (M_PI / 4.0)) / (M_PI / 4.0)) - 1.0;
	  srcx = mapMinusOneOneToImgCoord (phi, src[UP].width);
	  srcy = mapMinusOneOneToImgCoord (phi * lambda, src[UP].height);
	  break;

	case BACK:
	  if (lambda > 0)
	    {
	      lambda = 1.0 - ((lambda - (3.0 * (M_PI / 4.0))) / (M_PI / 4.0));
	      phi = ((phi - (M_PI / 4.0)) / (M_PI / 4.0)) - 1.0;
	      srcy = mapMinusOneOneToImgCoord (phi, src[UP].height);
	      srcx = mapMinusOneOneToImgCoord (phi * lambda, src[UP].width);
	    }
	  else
	    {
	      lambda = (lambda + M_PI) / (M_PI / 4.0);
	      phi = ((phi - (M_PI / 4.0)) / (M_PI / 4.0)) - 1.0;
	      srcy = mapMinusOneOneToImgCoord (phi, src[UP].height);
	      srcx = mapMinusOneOneToImgCoord (phi * -lambda, src[UP].width);
	    }
	  break;

	default:
	  exit (-1);
	}
      break;

    case DOWN:
      switch (identifyImageFromLambda (lambda))
	{
	case FRONT:
	  lambda = -lambda / (M_PI / 4.0);
	  phi = -1.0 - ((phi + (M_PI / 4.0)) / (M_PI / 4.0));
	  srcy = mapMinusOneOneToImgCoord (phi, src[DOWN].height);
	  srcx = mapMinusOneOneToImgCoord (-phi * lambda, src[DOWN].width);
	  break;

	case LEFT:
	  lambda = (lambda - (M_PI / 2.0)) / (M_PI / 4.0);
	  phi = -1.0 - ((phi + (M_PI / 4.0)) / (M_PI / 4.0));
	  srcx = mapMinusOneOneToImgCoord (phi, src[DOWN].width);
	  srcy = mapMinusOneOneToImgCoord (-phi * lambda, src[DOWN].height);
	  break;

	case RIGHT:
	  lambda = (lambda + (M_PI / 2.0)) / (M_PI / 4.0);
	  phi = 1.0 + ((phi + (M_PI / 4.0)) / (M_PI / 4.0));
	  srcx = mapMinusOneOneToImgCoord (phi, src[DOWN].width);
	  srcy = mapMinusOneOneToImgCoord (-phi * lambda, src[DOWN].height);
	  break;

	case BACK:
	  if (lambda > 0)
	    {
	      lambda = (lambda - M_PI) / (M_PI / 4.0);
	      phi = 1.0 + ((phi + (M_PI / 4.0)) / (M_PI / 4.0));
	      srcy = mapMinusOneOneToImgCoord (phi, src[DOWN].height);
	      srcx = mapMinusOneOneToImgCoord (phi * lambda, 
					       src[DOWN].width);
	      break;
	    }
	  else
	    {
	      lambda = (lambda + M_PI) / (M_PI / 4.0);
	      phi = 1.0 + ((phi + (M_PI / 4.0)) / (M_PI / 4.0));
	      srcy = mapMinusOneOneToImgCoord (phi, src[DOWN].height);
	      srcx = mapMinusOneOneToImgCoord (phi * lambda, 
					       src[DOWN].width);
	      break;
	    }

	default:
	  return genericPixel;
	}
      break;

    default:
      exit (-1);
    }
  
  return getPixel (&src[img], srcx, srcy);
}


void
distort (image    *dst, 
	 image    *src)
{
  int     i;
  int     dstx, dsty;
  char   *srcpixel;
  point   p;             /* cartesian coordinate */
  point   s;             /* spherical coordinate */

  for (i = 0; i < 6; i++)
    {
      printf ("Distorting image #%d (%dx%d)\n", 
	      i + 1,
	      dst[i].width,
	      dst[i].height);
      for (dsty = 0; dsty < dst[i].height; dsty++)
	{
	  for (dstx = 0; dstx < dst[i].width; dstx++)
	    {
	      switch (i)
		{
		case FRONT:
		  p[0] = 0.5;
		  p[1] = -0.5 * mapImgCoordToMinusOneOne (dstx, 
							  dst[i].width);
		  p[2] = -0.5 * mapImgCoordToMinusOneOne (dsty,
							  dst[i].height);
		  break;

		case LEFT:
		  p[0] = 0.5 * mapImgCoordToMinusOneOne (dstx,
							 dst[i].width);
		  p[1] = 0.5;
		  p[2] = -0.5 * mapImgCoordToMinusOneOne (dsty,
							  dst[i].height);
		  break;

		case BACK:
		  p[0] = -0.5;
		  p[1] = 0.5 * mapImgCoordToMinusOneOne (dstx,
							 dst[i].width);
		  p[2] = -0.5 * mapImgCoordToMinusOneOne (dsty,
							  dst[i].height);
		  break;

		case RIGHT:
		  p[0] = -0.5 * mapImgCoordToMinusOneOne (dstx,
							  dst[i].width);
		  p[1] = -0.5;
		  p[2] = -0.5 * mapImgCoordToMinusOneOne (dsty,
							  dst[i].height);
		  break;

		case UP:
		  p[0] = 0.5 * mapImgCoordToMinusOneOne (dsty,
							 dst[i].height);
		  p[1] = -0.5 * mapImgCoordToMinusOneOne (dstx,
							  dst[i].width);
		  p[2] = 0.5;
		  break;
		  
		case DOWN:
		  p[0] = -0.5 * mapImgCoordToMinusOneOne (dsty,
							  dst[i].height);
		  p[1] = -0.5 * mapImgCoordToMinusOneOne (dstx,
							  dst[i].width);
		  p[2] = -0.5;
		  break;		  

		default:
		  exit (-1);
		}

	      sphCoordInv (s, p);
	      srcpixel = mapSphericalToPixel (s, src);
	      setPixel (getPixel (&dst[i], dstx, dsty), srcpixel);
	    }
	}
    }
}