/* This file is part of the Pangolin Project.
 * http://github.com/stevenlovegrove/Pangolin
 *
 * Copyright (c) 2011 Steven Lovegrove
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#pragma once

#include <pangolin/gl/glinclude.h>
#include <pangolin/gl/glformattraits.h>
#include <pangolin/display/opengl_render_state.h>

#include <vector>
#include <math.h>

#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
#define USE_EIGEN
#endif

#ifdef USE_EIGEN
#include <Eigen/Core>
#include <Eigen/src/Geometry/AlignedBox.h>
#endif // USE_EIGEN

namespace pangolin
{

// h [0,360)
// s [0,1]
// v [0,1]
inline void glColorHSV( GLfloat hue, GLfloat s=1.0f, GLfloat v=1.0f )
{
    const GLfloat h = hue / 60.0f;
    const int i = (int)floor(h);
    const GLfloat f = (i%2 == 0) ? 1-(h-i) : h-i;
    const GLfloat m = v * (1-s);
    const GLfloat n = v * (1-s*f);
    switch(i)
    {
    case 0: glColor4f(v,n,m,1); break;
    case 1: glColor4f(n,v,m,1); break;
    case 2: glColor4f(m,v,n,1); break;
    case 3: glColor4f(m,n,v,1); break;
    case 4: glColor4f(n,m,v,1); break;
    case 5: glColor4f(v,m,n,1); break;
    default:
        break;
    }
}

inline void glColorBin( int bin, int max_bins, GLfloat sat=1.0f, GLfloat val=1.0f )
{
    if( bin >= 0 ) {
        const GLfloat hue = (GLfloat)(bin%max_bins) * 360.0f / (GLfloat)max_bins;
        glColorHSV(hue,sat,val);
    }else{
        glColor4f(1,1,1,1);
    }
}

template<typename T>
inline void glDrawVertices(
    size_t num_vertices, const T* const vertex_ptr, GLenum mode,
    size_t elements_per_vertex = GlFormatTraits<T>::components,
    size_t vertex_stride_bytes = 0 )
{
    if(num_vertices > 0)
    {
        PANGO_ENSURE(vertex_ptr != nullptr);
        PANGO_ENSURE(mode != GL_LINES || num_vertices % 2 == 0, "number of vertices (%) must be even in GL_LINES mode", num_vertices );

        glVertexPointer(elements_per_vertex, GlFormatTraits<T>::gltype, vertex_stride_bytes, vertex_ptr);
        glEnableClientState(GL_VERTEX_ARRAY);
        glDrawArrays(mode, 0, num_vertices);
        glDisableClientState(GL_VERTEX_ARRAY);
    }
}

template<typename TV, typename TC>
inline void glDrawColoredVertices(
    size_t num_vertices, const TV* const vertex_ptr, const TC* const color_ptr, GLenum mode,
    size_t elements_per_vertex = GlFormatTraits<TV>::components,
    size_t elements_per_color = GlFormatTraits<TC>::components,
    size_t vertex_stride_bytes = 0,
    size_t color_stride_bytes = 0
) {
    if(color_ptr) {
        glColorPointer(elements_per_color, GlFormatTraits<TC>::gltype, color_stride_bytes, color_ptr);
        glEnableClientState(GL_COLOR_ARRAY);
        glDrawVertices<TV>(num_vertices, vertex_ptr, mode, elements_per_vertex, vertex_stride_bytes);
        glDisableClientState(GL_COLOR_ARRAY);
    }else{
        glDrawVertices<TV>(num_vertices, vertex_ptr, mode, elements_per_vertex, vertex_stride_bytes);
    }
}

inline void glDrawLine( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2 )
{
    const GLfloat verts[] = { x1,y1,  x2,y2 };
    glDrawVertices<float>(2, verts, GL_LINES, 2);
}

inline void glDrawLine( GLfloat x1, GLfloat y1, GLfloat z1, GLfloat x2, GLfloat y2, GLfloat z2)
{
    const GLfloat verts[] = { x1,y1,z1,  x2,y2,z2 };
    glDrawVertices<float>(2, verts, GL_LINES, 3);
}

inline void glDrawCross( GLfloat x, GLfloat y, GLfloat rad )
{
    const GLfloat verts[] = { x-rad,y, x+rad, y, x,y-rad, x, y+rad};
    glDrawVertices<float>(4, verts, GL_LINES, 2);
}

inline void glDrawCross( GLfloat x, GLfloat y, GLfloat z, GLfloat rad )
{
    const GLfloat verts[] = { x-rad,y,z, x+rad,y,z, x,y-rad,z, x,y+rad,z, x,y,z-rad, x,y,z+rad };
    glDrawVertices<float>(6, verts, GL_LINES, 3);
}

inline void glDrawAxis(float s)
{
    const GLfloat cols[]  = { 1,0,0, 1,0,0, 0,1,0, 0,1,0, 0,0,1, 0,0,1 };
    const GLfloat verts[] = { 0,0,0, s,0,0, 0,0,0, 0,s,0, 0,0,0, 0,0,s };
    glDrawColoredVertices<float,float>(6, verts, cols, GL_LINES, 3, 3);
}

inline void glDrawRect( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2, GLenum mode = GL_TRIANGLE_FAN )
{
    const GLfloat verts[] = { x1,y1,  x2,y1,  x2,y2,  x1,y2 };
    glDrawVertices<float>(4, verts, mode, 2);
}

inline void glDrawRectPerimeter( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2 )
{
    glDrawRect(x1,y1, x2,y2, GL_LINE_LOOP);
}

inline void glDrawCirclePerimeter( float x, float y, float rad )
{
    const int N = 50;
    GLfloat verts[N*2];

    const float TAU_DIV_N = 2*(float)M_PI/N;
    for(int i = 0; i < N*2; i+=2) {
        verts[i] =   x + rad * cos(i*TAU_DIV_N);
        verts[i+1] = y + rad * sin(i*TAU_DIV_N);
    }

    glDrawVertices<float>(N, verts, GL_LINES, 2);
}

inline void glDrawCircle( GLfloat x, GLfloat y, GLfloat rad )
{    
    const int N = 50;
    GLfloat verts[N*2];
    
    // Draw vertices anticlockwise for front face
    const float TAU_DIV_N = 2*(float)M_PI/N;
    for(int i = 0; i < N*2; i+=2) {
        verts[i] =   x + rad * cos(-i*TAU_DIV_N);
        verts[i+1] = y + rad * sin(-i*TAU_DIV_N);
    }
    
    // Render filled shape and outline (to make it look smooth)
    glVertexPointer(2, GL_FLOAT, 0, verts);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_TRIANGLE_FAN, 0, N);
    glDrawArrays(GL_LINE_STRIP, 0, N);
    glDisableClientState(GL_VERTEX_ARRAY);
}

inline void glDrawColouredCube(GLfloat axis_min=-0.5f, GLfloat axis_max = +0.5f)
{
    const GLfloat l = axis_min;
    const GLfloat h = axis_max;
    
    const GLfloat verts[] = {
        l,l,h,  h,l,h,  l,h,h,  h,h,h,  // FRONT
        l,l,l,  l,h,l,  h,l,l,  h,h,l,  // BACK
        l,l,h,  l,h,h,  l,l,l,  l,h,l,  // LEFT
        h,l,l,  h,h,l,  h,l,h,  h,h,h,  // RIGHT
        l,h,h,  h,h,h,  l,h,l,  h,h,l,  // TOP
        l,l,h,  l,l,l,  h,l,h,  h,l,l   // BOTTOM
    };
    
    glVertexPointer(3, GL_FLOAT, 0, verts);
    glEnableClientState(GL_VERTEX_ARRAY);
    
    glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
    
    glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
    glDrawArrays(GL_TRIANGLE_STRIP, 8, 4);
    glDrawArrays(GL_TRIANGLE_STRIP, 12, 4);
    
    glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
    glDrawArrays(GL_TRIANGLE_STRIP, 16, 4);
    glDrawArrays(GL_TRIANGLE_STRIP, 20, 4);
    
    glDisableClientState(GL_VERTEX_ARRAY);
}

inline void glDraw_x0(GLfloat scale, int grid)
{
    const GLfloat maxord = grid*scale;
    for (int i = -grid; i <= grid; ++i) {
        glDrawLine(0.0, i*scale, -maxord, 0.0, i*scale, +maxord);
        glDrawLine(0.0, -maxord, i*scale, 0.0, +maxord, i*scale);
    }
}

inline void glDraw_y0(GLfloat scale, int grid)
{
    const GLfloat maxord = grid*scale;
    for (int i = -grid; i <= grid; ++i) {
        glDrawLine(i*scale, 0.0, -maxord, i*scale, 0.0, +maxord);
        glDrawLine(-maxord, 0.0, i*scale, +maxord, 0.0, i*scale);
    }
}

inline void glDraw_z0(GLfloat scale, int grid)
{
    const GLfloat maxord = grid*scale;
    for(int i=-grid; i<=grid; ++i ) {
        glDrawLine(i*scale,-maxord,   i*scale,+maxord);
        glDrawLine(-maxord, i*scale,  +maxord, i*scale);
    }
}

inline void glDrawFrustum( GLfloat u0, GLfloat v0, GLfloat fu, GLfloat fv, int w, int h, GLfloat scale )
{
    const GLfloat xl = scale * u0;
    const GLfloat xh = scale * (w*fu + u0);
    const GLfloat yl = scale * v0;
    const GLfloat yh = scale * (h*fv + v0);

    const GLfloat verts[] = {
        xl,yl,scale,  xh,yl,scale,
        xh,yh,scale,  xl,yh,scale,
        xl,yl,scale,  0,0,0,
        xh,yl,scale,  0,0,0,
        xl,yh,scale,  0,0,0,
        xh,yh,scale
    };

    glDrawVertices(11, verts, GL_LINE_STRIP, 3);
}

inline void glDrawTexture(GLenum target, GLuint texid)
{
    glBindTexture(target, texid);
    glEnable(target);
    
    const GLfloat sq_vert[] = { -1,-1,  1,-1,  1, 1,  -1, 1 };
    glVertexPointer(2, GL_FLOAT, 0, sq_vert);
    glEnableClientState(GL_VERTEX_ARRAY);   

    const GLfloat sq_tex[]  = { 0,0,  1,0,  1,1,  0,1  };
    glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
         
    glColor4f(1,1,1,1);
    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    glDisableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_TEXTURE_COORD_ARRAY);

    glDisable(target);
}

inline void glDrawTextureFlipY(GLenum target, GLuint texid)
{
    glBindTexture(target, texid);
    glEnable(target);
    
    const GLfloat sq_vert[] = { -1,-1,  1,-1,  1, 1,  -1, 1 };
    glVertexPointer(2, GL_FLOAT, 0, sq_vert);
    glEnableClientState(GL_VERTEX_ARRAY);   

    const GLfloat sq_tex[]  = { 0,1,  1,1,  1,0,  0,0  };
    glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
         
    glColor4f(1,1,1,1);
    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    glDisableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_TEXTURE_COORD_ARRAY);

    glDisable(target);
}


#ifdef USE_EIGEN

#ifndef HAVE_GLES
inline void glVertex( const Eigen::Vector3d& p )
{
    glVertex3dv(p.data());
}
#endif

inline void glDrawLine( const Eigen::Vector2d& p1, const Eigen::Vector2d& p2 )
{
    glDrawLine((GLfloat)p1(0), (GLfloat)p1(1), (GLfloat)p2(0), (GLfloat)p2(1));
}

// Draws a vector of 2d or 3d vertices using provided ``mode``.
//
// Preconditions:
//  - ``mode`` must be GL_POINTS, GL_LINES, GL_LINE_STRIP, GL_LINE_LOOP, etc
//  - If ``mode == GL_LINES``, then ``vertices.size()`` must be a multiple of 2.
//
template<typename P, int N, class Allocator>
void glDrawVertices(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices, GLenum mode)
{
    glDrawVertices(vertices.size(), vertices.data(), mode);
}

// Draws a vector of 2d or 3d points.
//
template<typename P, int N, class Allocator>
void glDrawPoints(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
{
    glDrawVertices(vertices, GL_POINTS);
}

// Draws a vector of 2d or 3d lines.
//
//  Precondition: ``vertices.size()`` must be a multiple of 2.
//
template<typename P, int N, class Allocator>
void glDrawLines(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
{
    glDrawVertices(vertices, GL_LINES);
}

// Draws a 2d or 3d line strip.
//
template<typename P, int N, class Allocator>
void glDrawLineStrip(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
{
    glDrawVertices(vertices, GL_LINE_STRIP);
}

// Draws a 2d or 3d line loop.
//
template<typename P, int N, class Allocator>
void glDrawLineLoop(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
{
    glDrawVertices(vertices, GL_LINE_LOOP);
}

inline void glDrawCross( const Eigen::Vector2d& p, double r = 5.0 )
{
    glDrawCross((GLfloat)p(0), (GLfloat)p(1), (GLfloat)r);
}

inline void glDrawCross( const Eigen::Vector3d& p, double r = 5.0 )
{
    glDrawCross((GLfloat)p(0), (GLfloat)p(1), (GLfloat)p(2), (GLfloat)r);
}

inline void glDrawCircle( const Eigen::Vector2d& p, double radius = 5.0 )
{
    glDrawCircle((GLfloat)p(0), (GLfloat)p(1), (GLfloat)radius);
}

inline void glDrawCirclePerimeter( const Eigen::Vector2d& p, double radius = 5.0 )
{
    glDrawCirclePerimeter((GLfloat)p(0), (GLfloat)p(1), (GLfloat)radius);
}

inline void glSetFrameOfReference( const Eigen::Matrix4f& T_wf )
{
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glMultMatrixf( T_wf.data() );
}

inline void glSetFrameOfReference( const Eigen::Matrix4d& T_wf )
{
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
#ifndef HAVE_GLES
    glMultMatrixd( T_wf.data() );
#else
    const Eigen::Matrix4f fT_wf = T_wf.cast<GLfloat>();
    glMultMatrixf( fT_wf.data() );
#endif
}

inline void glSetFrameOfReference( const pangolin::OpenGlMatrix& T_wf )
{
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glMultMatrixd( T_wf.m );
}

inline void glUnsetFrameOfReference()
{
    glPopMatrix();
}

template<typename T, typename S>
inline void glDrawAxis( const T& T_wf, S scale )
{
    glSetFrameOfReference(T_wf);
    glDrawAxis(scale);
    glUnsetFrameOfReference();
}

template<typename T>
inline void glDrawFrustum( const Eigen::Matrix<T,3,3>& Kinv, int w, int h, GLfloat scale )
{
    glDrawFrustum((GLfloat)Kinv(0,2), (GLfloat)Kinv(1,2), (GLfloat)Kinv(0,0), (GLfloat)Kinv(1,1), w, h, scale);
}

template<typename T>
inline void glDrawFrustum( const Eigen::Matrix<T,3,3>& Kinv, int w, int h, const Eigen::Matrix<T,4,4>& T_wf, T scale )
{
    glSetFrameOfReference(T_wf);
    glDrawFrustum(Kinv,w,h,scale);
    glUnsetFrameOfReference();
}

template<typename T>
inline void glDrawAlignedBox( const Eigen::AlignedBox<T,2>& box, GLenum mode = GL_TRIANGLE_FAN )
{
    const Eigen::Matrix<float,2,1> l = box.min().template cast<float>();
    const Eigen::Matrix<float,2,1> h = box.max().template cast<float>();

    GLfloat verts[] = {
        l[0], l[1],
        h[0], l[1],
        h[0], h[1],
        l[0], h[1]
    };

    glDrawVertices(4, verts, mode, 2);
}

template<typename T>
inline void glDrawAlignedBoxPerimeter( const Eigen::AlignedBox<T,2>& box)
{
    glDrawAlignedBox<T>(box, GL_LINE_LOOP);
}

template<typename T>
inline void glDrawAlignedBox( const Eigen::AlignedBox<T,3>& box)
{
    const Eigen::Matrix<float,3,1> l = box.min().template cast<float>();
    const Eigen::Matrix<float,3,1> h = box.max().template cast<float>();

    GLfloat verts[] = {
        l[0], l[1], l[2],
        l[0], l[1], h[2],
        h[0], l[1], h[2],
        h[0], l[1], l[2],
        l[0], l[1], l[2],
        l[0], h[1], l[2],
        h[0], h[1], l[2],
        h[0], l[1], l[2],
        h[0], h[1], l[2],
        h[0], h[1], h[2],
        l[0], h[1], h[2],
        l[0], h[1], l[2],
        l[0], h[1], h[2],
        l[0], l[1], h[2],
        h[0], l[1], h[2],
        h[0], h[1], h[2]
    };

    glDrawVertices(16, verts, GL_LINE_STRIP, 3);
}

#endif // USE_EIGEN

#ifndef HAVE_GLES
inline void glPixelTransferScale( float r, float g, float b )
{
    glPixelTransferf(GL_RED_SCALE,r);
    glPixelTransferf(GL_GREEN_SCALE,g);
    glPixelTransferf(GL_BLUE_SCALE,b);
}

inline void glPixelTransferScale( float scale )
{
    glPixelTransferScale(scale,scale,scale);
}
#endif

void glRecordGraphic(float x, float y, float radius);

}