#define HMAP_SIZE 1024


typedef struct {
  double x, y, z;
  double nx, ny, nz;
} Vertex;

typedef struct {
  double (*map)[HMAP_SIZE];
  double (*old)[HMAP_SIZE];
  double maps[2][HMAP_SIZE][HMAP_SIZE];
  double normals[HMAP_SIZE][HMAP_SIZE][3];
  double stepXY;
  double stepZ;
  double color[4];
  GLuint buf;
  int bufCount;
} HMap;


void hmapInit(HMap *hmap, double stepXY, double stepZ, double r, double g, double b, double a) {
  hmap->map = hmap->maps[0];
  hmap->old = hmap->maps[1];
  for(int i = 0; i < HMAP_SIZE; ++i) {
    for(int j = 0; j < HMAP_SIZE; ++j) {
      hmap->map[i][j] = 0;
      hmap->old[i][j] = 0;
      hmap->normals[i][j][0] = 0;
      hmap->normals[i][j][1] = 0;
      hmap->normals[i][j][2] = 0;
    }
  }
  hmap->stepXY = stepXY;
  hmap->stepZ = stepZ;
  hmap->color[0] = r;
  hmap->color[1] = g;
  hmap->color[2] = b;
  hmap->color[3] = a;

  hmap->buf = 0;
  hmap->bufCount = 0;
  glGenBuffers(1, &hmap->buf);
}


void hmapDeinit(HMap *hmap) {
  glDeleteBuffers(1, &hmap->buf);
}


void hmapSwap(HMap *hmap) {
  double (*m)[HMAP_SIZE] = hmap->map;
  hmap->map = hmap->old;
  hmap->old = m;
}


void hmapCopy(HMap *hmap) {
  for(int i = 0; i < HMAP_SIZE; ++i)
    for(int j = 0; j < HMAP_SIZE; ++j)
      hmap->old[i][j] = hmap->map[i][j];
}


void hmapGetVertex(const HMap *hmap, int i, int j, Vertex *v) {
  int ii = (i%HMAP_SIZE + HMAP_SIZE)%HMAP_SIZE;
  int jj = (j%HMAP_SIZE + HMAP_SIZE)%HMAP_SIZE;
  v->x = hmap->stepXY*(i - 0.5*HMAP_SIZE);
  v->y = hmap->stepXY*(j - 0.5*HMAP_SIZE);
  v->z = hmap->stepZ*hmap->map[ii][jj];
  v->nx = hmap->normals[ii][jj][0];
  v->ny = hmap->normals[ii][jj][1];
  v->nz = hmap->normals[ii][jj][2];
}


void hmapBuildBuf(HMap *hmap) {
  static Vertex buf[ (HMAP_SIZE*2 + 4)*HMAP_SIZE*10 ];
  Vertex *v = buf;
  for(int j = 0; j < HMAP_SIZE; ++j) {
    hmapGetVertex(hmap, 0, j, v++);
    for(int i = 0; i <= HMAP_SIZE; ++i) {
      hmapGetVertex(hmap, i, j, v++);
      hmapGetVertex(hmap, i, j+1, v++);
    }
    hmapGetVertex(hmap, HMAP_SIZE, j+1, v++);
  }
  hmap->bufCount = v - buf;

  glBindBuffer(GL_ARRAY_BUFFER, hmap->buf);
  glBufferData(GL_ARRAY_BUFFER, hmap->bufCount*sizeof(*v), buf, GL_STATIC_DRAW);
  glBindBuffer(GL_ARRAY_BUFFER, 0);
}


void hmapDraw(const HMap *hmap) {
  glColor4dv(hmap->color);

  glBindBuffer(GL_ARRAY_BUFFER, hmap->buf);
  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_NORMAL_ARRAY);
  glVertexPointer(3, GL_DOUBLE, sizeof(Vertex), (const void*)offsetof(Vertex, x));
  glNormalPointer(GL_DOUBLE, sizeof(Vertex), (const void*)offsetof(Vertex, nx));

  glDrawArrays(GL_TRIANGLE_STRIP, 0, hmap->bufCount);

  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_NORMAL_ARRAY);
  glBindBuffer(GL_ARRAY_BUFFER, 0);

  glColor4d(1, 1, 1, 1);
}


void hmapDrawWrap(const HMap *hmap) {
  for(int i = -1; i <= 1; ++i) {
    for(int j = -1; j <= 1; ++j) {
      glPushMatrix();
      glTranslated(HMAP_SIZE*i*hmap->stepXY, HMAP_SIZE*j*hmap->stepXY, 0);
      hmapDraw(hmap);
      glPopMatrix();
    }
  }
}

double hmapGet(const HMap *hmap, int i, int j) {
  i = (i%HMAP_SIZE + HMAP_SIZE)%HMAP_SIZE;
  j = (j%HMAP_SIZE + HMAP_SIZE)%HMAP_SIZE;
  return hmap->map[i][j];
}


void hmapCalcNormals(HMap *hmap) {
  double dxy2 = 4 * hmap->stepXY * hmap->stepXY;
  for(int i = 0; i < HMAP_SIZE; ++i) {
    for(int j = 0; j < HMAP_SIZE; ++j) {
      double dzx = (hmapGet(hmap, i+1, j) - hmapGet(hmap, i-1, j))*hmap->stepZ;
      double k = 1/sqrt(dzx*dzx + dxy2);
      double nz = dxy2*k;
      double nx = -dzx*k;

      double dzy = (hmapGet(hmap, i, j+1) - hmapGet(hmap, i, j-1))*hmap->stepZ;
      k = 1/sqrt(dzy*dzy + dxy2);
      nz += dxy2*k;
      double ny = -dzy*k;

      k = 1/sqrt(nx*nx + ny*ny + nz*nz);
      hmap->normals[i][j][0] = nx*k;
      hmap->normals[i][j][1] = ny*k;
      hmap->normals[i][j][2] = nz*k;
    }
  }
}


double cubicInterpolation(double x0, double x1, double x2, double x3, float l) {
    double a = x3 - x2 + x1 - x0;
    double b = x0 - x1 - a;
    double c = x2 - x0;
    double d = x1;
    return ((a*l + b)*l + c)*l + d;
}


double hmapGetBicubic(HMap *hmap, double x, double y, int step) {
  x /= step;
  y /= step;
  int i0 = (int)floor(x);
  int j0 = (int)floor(y);
  double dx = x - i0;
  double dy = y - j0;

  double m[4][4];
  for(int i = 0; i < 4; ++i)
    for(int j = 0; j < 4; ++j)
      m[i][j]= hmapGet(hmap, (i0 + i - 1)*step, (j0 + j - 1)*step);

  return cubicInterpolation(
    cubicInterpolation(m[0][0], m[1][0], m[2][0], m[3][0], dx),
    cubicInterpolation(m[0][1], m[1][1], m[2][1], m[3][1], dx),
    cubicInterpolation(m[0][2], m[1][2], m[2][2], m[3][2], dx),
    cubicInterpolation(m[0][3], m[1][3], m[2][3], m[3][3], dx),
    dy );
}


void hmapGenerate(HMap *hmap) {
  const int maxStep = HMAP_SIZE/2;
  const int minStep = 1;
  const double maxFactor = 0.25*maxStep;

  for(int i = 0; i < HMAP_SIZE; i += maxStep)
    for(int j = 0; j < HMAP_SIZE; j += maxStep)
        hmap->map[i][j] = (randomFloat() - 0.5)*2*maxFactor;

  double factor = 0.5*maxFactor;
  for(int step = maxStep/2; step >= minStep; step /= 2, factor /= 2) {
    for(int i = step; i < HMAP_SIZE; i += 2*step)
      for(int j = step; j < HMAP_SIZE; j += 2*step)
        hmap->map[i][j] = ( hmapGet(hmap, i - step, j - step)
                          + hmapGet(hmap, i + step, j - step)
                          + hmapGet(hmap, i - step, j + step)
                          + hmapGet(hmap, i + step, j + step) )*0.25
                        + (randomFloat() - 0.5)*2*factor;
    for(int i = 0; i < HMAP_SIZE; i += step)
      for(int j = step*(1 - i/step%2); j < HMAP_SIZE; j += 2*step)
        hmap->map[i][j] = ( hmapGet(hmap, i - step, j)
                          + hmapGet(hmap, i + step, j)
                          + hmapGet(hmap, i, j - step)
                          + hmapGet(hmap, i, j + step) )*0.25
                        + (randomFloat() - 0.5)*2*factor;
  }

  if (minStep > 1)
    for(int i = 0; i < HMAP_SIZE; ++i)
      for(int j = 0; j < HMAP_SIZE; ++j)
        if (i % minStep || j % minStep)
          hmap->map[i][j] = hmapGetBicubic(hmap, i, j, minStep);

  hmapCalcNormals(hmap);
}