#include <iostream>
#include <vector>
#include <cmath>
#include <fstream>
#include <iomanip>

class HeightMap
{
public:
    HeightMap(int width, float widthScale, float heightScale,
             const std::string& rawFilename, bool is16bit = true)
        : m_Width(width), m_WidthScale(widthScale), m_HeightScale(heightScale)
    {
        std::ifstream ifs;
    
        // open the file (should add some error checking here)
        ifs.open(rawFilename.c_str(), std::ios::binary);

        // create the new vertex array
        m_Vertices = new float[m_Width * m_Width];

        unsigned short tmp = 0;
        float invScale = 1.0f / 65535.0f;

        // loop through and populate the vertices
        for(int y = 0; y < m_Width; ++y)
        {
            for(int x = 0; x < m_Width; ++x)
            {
                // get first byte
                tmp = ifs.get();
                // if its 16 bit, get the second
                if(is16bit)
                    tmp += ifs.get() << 8;

                // add the current value to the correct position
                m_Vertices[y*m_Width + x] = tmp * invScale;
            }
        }
                
        ifs.close();
    }//end HeightMap()

    ~HeightMap() 
    {
        delete[] m_Vertices;
    }//end ~HeightMap

    /**
     * Print the height map to standard output, useful for debugging
     */
    void printHeightMap()
    {
        // now print out the heightmap
        for(int y = 0; y < m_Width; ++y)
        {
            for(int x = 0; x < m_Width; ++x)
            {
                std::cout << std::setw(2) << 
                    static_cast<int>(m_Vertices[y*m_Width + x] * heightScale)<< ",";
            }
            std::cout << std::endl;
        }
    }//end printHeightMap

    /**
     * This function is merely a modified form of the function
     * of the same name in TerrainRenderable.
     */
    float getHeightAt(float x, float z)
    {
        float start_x = 0;
        float start_y = indexHeight(0,0);
        float start_z = 0;
        float end_x = m_Width*m_WidthScale/4;
        float end_y = indexHeight(m_Width,m_Width);
        float end_z = m_Width*m_WidthScale/4;

        float x_pct = ( x - start_x ) / ( end_x - start_x );
        float z_pct = ( z - start_z ) / ( end_z - start_z );

        float x_pt = x_pct * ( float ) ( m_Width);
        float z_pt = z_pct * ( float ) ( m_Width);

        int x_index = ( int ) x_pt;
        int z_index = ( int ) z_pt;

        // If we got to the far right / bottom edge, move one back
        if (x_index == m_Width)
        {
            --x_index;
            x_pct = 1.0f;
        }
        else
        {
            // get remainder
            x_pct = x_pt - x_index;
        }
        if (z_index == m_Width)
        {
            --z_index;
            z_pct = 1.0f;
        }
        else
        {
            z_pct = z_pt - z_index;
        }

        //bilinear interpolate to find the height.
        float t1 = indexHeight( x_index, z_index);
        float t2 = indexHeight( x_index + 1, z_index);
        float b1 = indexHeight( x_index, z_index + 1);
        float b2 = indexHeight( x_index + 1, z_index + 1);

        float midpoint = (b1 + t2) / 2.0;

        if (x_pct + z_pct <= 1) {
            b2 = midpoint + (midpoint - t1);
        } else {
            t1 = midpoint + (midpoint - b2);
        }

        float t = ( t1 * ( 1 - x_pct ) ) + ( t2 * ( x_pct ) );
        float b = ( b1 * ( 1 - x_pct ) ) + ( b2 * ( x_pct ) );

        float h = ( t * ( 1 - z_pct ) ) + ( b * ( z_pct ) );

        return h * m_HeightScale;
    }//end getHeightAt

private:
    /**
     * Simple function for pulling out a value from the array
     */
    inline float indexHeight(int x, int y)
    {
        return m_Vertices[m_Width * y + x];
    }//end indexHeight

private:
    float* m_Vertices;
    int m_Width;
    float m_WidthScale;
    float m_HeightScale;
};