// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.


//---------------------------------
//  Universal Image Processing Algorithm (UIPA)
//   _  _  __  ___  __
//  ( )( )(  )(  ,\(  )
//  ( )( ) )(  ) _//__\
//  (____)(__)(_) (_)(_)
//-------
// Generic Image algorithm that require
//  an ImageModel class that must implement the following:
//
// ::Contains(int y, int x) <= Tell if a point is inside or not the image
// ::operator(int y,int x)  <= Modification accessor over the pixel (y,x)
// ::Width()
// ::Height()

#ifndef OPENMVG_IMAGE_IMAGE_DRAWING_HPP
#define OPENMVG_IMAGE_IMAGE_DRAWING_HPP

#include "openMVG/image/image_container.hpp"

namespace openMVG {
namespace image {

/// Put the pixel in the image to the given color only if the point (xc,yc)
///  is inside the image.
/// /!\ Be careful at the order (Y,X).
template <typename Image, typename Color>
inline void SafePutPixel(int yc, int xc, const Color& col, Image *pim)
{
  if (pim) {
    if (pim->Contains(yc, xc))
      (*pim)(yc, xc) = col;
  }
}

// Bresenham approach to draw ellipse.
// http://raphaello.univ-fcomte.fr/ig/algorithme/ExemplesGLUt/BresenhamEllipse.htm
// Add the rotation of the ellipse.
// As the algo. use symmetry we must use 4 rotations.
template <typename Image, typename Color>
void DrawEllipse(int xc, int yc, int radiusA, int radiusB,
                 const Color& col, Image *pim, double angle = 0.0)
{
  int a = radiusA, b = radiusB;

  // Counter Clockwise rotation matrix.
  double matXY[4] = { cos(angle), sin(angle),
                     -sin(angle), cos(angle)};
  int x, y;
  double d1, d2;
  x = 0;
  y = b;
  d1 = b*b - a*a*b + a*a/4;

  int rotX = ceil(matXY[0] * x + matXY[1] * y);
  int rotY = ceil(matXY[2] * x + matXY[3] * y);
  SafePutPixel( yc + rotY, xc + rotX, col, pim);
  rotX = matXY[0] * x - matXY[1] * y;
  rotY = matXY[2] * x - matXY[3] * y;
  SafePutPixel( yc + rotY, xc + rotX, col, pim);
  rotX = -matXY[0] * x - matXY[1] * y;
  rotY = -matXY[2] * x - matXY[3] * y;
  SafePutPixel( yc + rotY, xc + rotX, col, pim);
  rotX = -matXY[0] * x + matXY[1] * y;
  rotY = -matXY[2] * x + matXY[3] * y;
  SafePutPixel( yc + rotY, xc + rotX, col, pim);

  while ( a*a*(y-.5) > b*b*(x+1) ) {
    if ( d1 < 0 ) {
      d1 += b*b*(2*x+3);
      ++x;
    }
    else  {
      d1 += b*b*(2*x+3) + a*a*(-2*y+2);
      ++x;
      --y;
    }
    rotX = matXY[0] * x + matXY[1] * y;
    rotY = matXY[2] * x + matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = matXY[0] * x - matXY[1] * y;
    rotY = matXY[2] * x - matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = -matXY[0] * x - matXY[1] * y;
    rotY = -matXY[2] * x - matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = -matXY[0] * x + matXY[1] * y;
    rotY = -matXY[2] * x + matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
  }
  d2 = b*b*(x+.5)*(x+.5) + a*a*(y-1)*(y-1) - a*a*b*b;
  while ( y > 0 ) {
    if ( d2 < 0 ) {
      d2 += b*b*(2*x+2) + a*a*(-2*y+3);
      --y;
      ++x;
    }
    else {
      d2 += a*a*(-2*y+3);
      --y;
    }
    rotX = matXY[0] * x + matXY[1] * y;
    rotY = matXY[2] * x + matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = matXY[0] * x - matXY[1] * y;
    rotY = matXY[2] * x - matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = -matXY[0] * x - matXY[1] * y;
    rotY = -matXY[2] * x - matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
    rotX = -matXY[0] * x + matXY[1] * y;
    rotY = -matXY[2] * x + matXY[3] * y;
    SafePutPixel( yc + rotY, xc + rotX, col, pim);
  }
}

// Bresenham approach do not allow to draw concentric circle without holes.
// So it's better the use the Andres method.
// http://fr.wikipedia.org/wiki/Algorithme_de_tracé_de_cercle_d'Andres.
template <typename Image, typename Color>
void DrawCircle(int x, int y, int radius, const Color& col, Image *pim)
{
  Image &im = *pim;
  if (  im.Contains(y + radius, x + radius)
     || im.Contains(y + radius, x - radius)
     || im.Contains(y - radius, x + radius)
     || im.Contains(y - radius, x - radius)) {
    int x1 = 0;
    int y1 = radius;
    int d = radius - 1;
    while (y1 >= x1) {
      // Draw the point for each octant.
      SafePutPixel( y1 + y,  x1 + x, col, pim);
      SafePutPixel( x1 + y,  y1 + x, col, pim);
      SafePutPixel( y1 + y, -x1 + x, col, pim);
      SafePutPixel( x1 + y, -y1 + x, col, pim);
      SafePutPixel(-y1 + y,  x1 + x, col, pim);
      SafePutPixel(-x1 + y,  y1 + x, col, pim);
      SafePutPixel(-y1 + y, -x1 + x, col, pim);
      SafePutPixel(-x1 + y, -y1 + x, col, pim);
      if (d >= 2 * x1) {
        d = d - 2 * x1 - 1;
        x1 += 1;
      } else {
        if (d <= 2 * (radius - y1)) {
          d = d + 2 * y1 - 1;
          y1 -= 1;
        } else  {
          d = d + 2 * (y1 - x1 - 1);
          y1 -= 1;
          x1 += 1;
        }
      }
    }
  }
}

// Bresenham algorithm
template <typename Image, typename Color>
void DrawLine(int xa, int ya, int xb, int yb, const Color& col, Image *pim)
{
  Image &im = *pim;

  // If one point is outside the image
  // Replace the outside point by the intersection of the line and
  // the limit (either x=width or y=height).
  if (!im.Contains(ya, xa) || !im.Contains(yb, xb)) {

    int width = pim->Width(), height = pim->Height();
    const bool xdir = xa<xb, ydir = ya<yb;
    float nx0 = float(xa), nx1 = float(xb), ny0 = float(ya), ny1 = float(yb),
        &xleft = xdir?nx0:nx1,  &yleft = xdir?ny0:ny1,
        &xright = xdir?nx1:nx0, &yright = xdir?ny1:ny0,
        &xup = ydir?nx0:nx1,    &yup = ydir?ny0:ny1,
        &xdown = ydir?nx1:nx0,  &ydown = ydir?ny1:ny0;

    if (xright<0 || xleft>=width) return;
    if (xleft<0) {
      yleft -= xleft*(yright - yleft)/(xright - xleft);
      xleft  = 0;
    }
    if (xright>=width) {
      yright -= (xright - width)*(yright - yleft)/(xright - xleft);
      xright  = float(width) - 1;
    }
    if (ydown<0 || yup>=height) return;
    if (yup<0) {
      xup -= yup*(xdown - xup)/(ydown - yup);
      yup  =  0;
    }
    if (ydown>=height) {
      xdown -= (ydown - height)*(xdown - xup)/(ydown - yup);
      ydown  =  float(height) - 1;
    }

    xa = (int) xleft;    xb = (int) xright;
    ya = (int) yleft;    yb = (int) yright;
  }

  int xbas, xhaut, ybas, yhaut;
  // Check the condition ybas < yhaut.
  if (ya <= yb) {
    xbas = xa;    ybas = ya;
    xhaut = xb;   yhaut = yb;
  } else {
    xbas = xb;    ybas = yb;
    xhaut = xa;   yhaut = ya;
  }
  // Initialize slope.
  int x, y, dx, dy, incrmX, incrmY, dp, N, S;
  dx = xhaut - xbas;
  dy = yhaut - ybas;
  if (dx > 0) { // If xhaut > xbas we will increment X.
    incrmX = 1;
  } else {
    incrmX = -1; // else we will decrement X.
    dx *= -1;
  }
  if (dy > 0) { // Positive slope will increment X.
    incrmY = 1;
  } else {      // Negative slope.
    incrmY = -1;
  }
  if (dx >= dy) {
    dp = 2 * dy - dx;
    S = 2 * dy;
    N = 2 * (dy - dx);
    y = ybas;
    x = xbas;
    while (x != xhaut) {
      SafePutPixel( y, x, col, pim);
      x += incrmX;
      if (dp <= 0) { // Go in direction of the South Pixel.
        dp += S;
      } else {       // Go to the North.
        dp += N;
        y+=incrmY;
      }
    }
  } else {
    dp = 2 * dx - dy;
    S = 2 * dx;
    N = 2 * (dx - dy);
    x = xbas;
    y = ybas;
    while (y < yhaut) {
      SafePutPixel( y, x, col, pim);
      y += incrmY;
      if (dp <= 0) { // Go in direction of the South Pixel.
        dp += S;
      } else {       // Go to the North.
        dp += N;
        x += incrmX;
      }
    }
  }
  SafePutPixel( y, x, col, pim);
}

// Filled circle
// Exterior point computed with bresenham approach
// i.e: DrawCircle
template <typename Image, typename Color>
void FilledCircle(int x, int y, int radius, const Color& col, Image *pim)
{
  Image &im = *pim;
  if (  im.Contains(y + radius, x + radius)
     || im.Contains(y + radius, x - radius)
     || im.Contains(y - radius, x + radius)
     || im.Contains(y - radius, x - radius)) {
    int x1 = 0;
    int y1 = radius;
    int d = radius - 1;
    while (y1 >= x1) {
      DrawLine(x1 + x, y1 + y, x1 + x, -y1 + y, col, pim);
      DrawLine(y1 + x, x1 + y, y1 + x, -x1 + y, col, pim);
      DrawLine(-x1 + x, y1 + y, -x1 + x, -y1 + y, col, pim);
      DrawLine(-y1 + x, x1 + y, -y1 + x, -x1 + y, col, pim);
      if (d >= 2 * x1) {
        d = d - 2 * x1 - 1;
        x1 += 1;
      } else {
        if (d <= 2 * (radius - y1)) {
          d = d + 2 * y1 - 1;
          y1 -= 1;
        } else  {
          d = d + 2 * (y1 - x1 - 1);
          y1 -= 1;
          x1 += 1;
        }
      }
    }
  }
}

// Draw a serie of circles along the line, the algorithm is slow but accurate
template <typename Image, typename Color>
void DrawLineThickness(int xa, int ya, int xb, int yb, const Color& col, int thickness, Image *pim)
{
  Image &im = *pim;
  int halfThickness = (thickness+1)/2;

  // If one point is outside the image
  // Replace the outside point by the intersection of the line and
  // the limit (either x=width or y=height).
  if (!im.Contains(ya, xa) || !im.Contains(yb, xb)) {

    int width = pim->Width(), height = pim->Height();
    const bool xdir = xa<xb, ydir = ya<yb;
    float nx0 = float(xa), nx1 = float(xb), ny0 = float(ya), ny1 = float(yb),
        &xleft = xdir?nx0:nx1,  &yleft = xdir?ny0:ny1,
        &xright = xdir?nx1:nx0, &yright = xdir?ny1:ny0,
        &xup = ydir?nx0:nx1,    &yup = ydir?ny0:ny1,
        &xdown = ydir?nx1:nx0,  &ydown = ydir?ny1:ny0;

    if (xright<0 || xleft>=width) return;
    if (xleft<0) {
      yleft -= xleft*(yright - yleft)/(xright - xleft);
      xleft  = 0;
    }
    if (xright>=width) {
      yright -= (xright - width)*(yright - yleft)/(xright - xleft);
      xright  = float(width) - 1;
    }
    if (ydown<0 || yup>=height) return;
    if (yup<0) {
      xup -= yup*(xdown - xup)/(ydown - yup);
      yup  =  0;
    }
    if (ydown>=height) {
      xdown -= (ydown - height)*(xdown - xup)/(ydown - yup);
      ydown  =  float(height) - 1;
    }

    xa = (int) xleft;    xb = (int) xright;
    ya = (int) yleft;    yb = (int) yright;
  }

  int xbas, xhaut, ybas, yhaut;
  // Check the condition ybas < yhaut.
  if (ya <= yb) {
    xbas = xa;    ybas = ya;
    xhaut = xb;   yhaut = yb;
  } else {
    xbas = xb;    ybas = yb;
    xhaut = xa;   yhaut = ya;
  }
  // Initialize slope.
  int x, y, dx, dy, incrmX, incrmY, dp, N, S;
  dx = xhaut - xbas;
  dy = yhaut - ybas;
  if (dx > 0) { // If xhaut > xbas we will increment X.
    incrmX = 1;
  } else {
    incrmX = -1; // else we will decrement X.
    dx *= -1;
  }
  if (dy > 0) { // Positive slope will increment X.
    incrmY = 1;
  } else {      // Negative slope.
    incrmY = -1;
  }
  if (dx >= dy) {
    dp = 2 * dy - dx;
    S = 2 * dy;
    N = 2 * (dy - dx);
    y = ybas;
    x = xbas;
    while (x != xhaut) {
      DrawCircle(x, y, halfThickness, col, pim);
      x += incrmX;
      if (dp <= 0) { // Go in direction of the South Pixel.
        dp += S;
      } else {       // Go to the North.
        dp += N;
        y+=incrmY;
      }
    }
  } else {
    dp = 2 * dx - dy;
    S = 2 * dx;
    N = 2 * (dx - dy);
    x = xbas;
    y = ybas;
    while (y < yhaut) {
      DrawCircle(x, y, halfThickness, col, pim);
      y += incrmY;
      if (dp <= 0) { // Go in direction of the South Pixel.
        dp += S;
      } else {       // Go to the North.
        dp += N;
        x += incrmX;
      }
    }
  }
  DrawCircle(x, y, halfThickness, col, pim);
}

} // namespace image
} // namespace openMVG

#endif  // OPENMVG_IMAGE_IMAGE_DRAWING_HPP
