/*************************************************************************/
/* File:        project8.c                                               */
/* Description: User project #8 - harmonic function code                 */
/* Author:      Rod Grupen                                               */
/* Date:        12-2014                                                  */
/*************************************************************************/
#include <math.h>
#include <stdio.h>
#include "Xkw/Xkw.h"

#include "roger.h"
#include "simulate.h"
#include "control.h"
#include "modes.h"

/************************************************************************/
/************************************************************************/

// what happens if you change the step size?
#define PATH_STEPSIZE 0.2

void dilate_obstacles(roger, radius)
Robot * roger;
double radius;
{
  // scan the map
  //    if you find a DILATED_OBSTACLE, re-label it FREESPACE
  double r_bin_x, r_bin_y;
  r_bin_x = (radius/XDELTA_DEV);
  r_bin_y = (radius/YDELTA_DEV);
  int row, col, i, j;

  for (row=0; row<NBINS; ++row) {
    for (col=0; col<NBINS; ++col) {
      if(roger->world_map.occupancy_map[row][col] == DILATED_OBSTACLE) {
        roger->world_map.occupancy_map[row][col] = FREESPACE;
      }
    }
  }

  // scan the map,
  for (row=0; row<NBINS; ++row) {
    for (col=0; col<NBINS; ++col) {

  //    if you find an OBSTACLE {
  //       scan the local neighborhood (to a radius you define)
  //               /* overly cautious? round up the radius---use radius = ceil(radius);
  //          if you find A FREESPACE node && sqrt(dx*dx+dy*dy)<radius {
  //             re-label it DILATED_OBSTACLE,
  //             set its potential to 1.0 (obstacle potential)
  //             define its color (try LIGHTYELLOW)

    }
  }
}

void sor(roger)
Robot * roger;
{
  int i, j, sor_count=0, converged = FALSE;
  double sor_once();

  // what's the impact of changing 5000 to 100?
  while (!converged && (sor_count < 5000)) {
    ++sor_count;
    if (sor_once(roger) < THRESHOLD) converged = TRUE;
  }
  if (sor_count > 1)
    printf("completed harmonic function --- %d iterations\n", sor_count);
}

// one complete backup, only dirichlet boundary conditions
double sor_once(roger)
Robot * roger;
{
  int i, j, ipos, ineg, jpos, jneg;
  double residual, max, front, back, up, down;

  // iterate over entire map once
  // return the  maximum change in the potential value over the entire
  //    occupancy map as a means of determining convergence
  max = 0.0;
  for (i = 0; i < NBINS; ++i) {
    for (j = 0; j < NBINS; ++j) {
      if (roger->world_map.occupancy_map[i][j] == FREESPACE) {
//       residual = ...
//       roger->world_map.potential_map[i][j] = ...
//          printf("SOR:  potential(%d,%d)=%f\n",
//             i,j,roger->world_map.potential_map[i][j]);
      	if (fabs(residual) > max) max = fabs(residual);
      }
    }
  }
  return(max);
}

double compute_gradient(x, y, roger, grad)
double x, y;
Robot * roger;
double grad[2]; // grad = [ d(phi)/dx  d(phi)/dy ]
{
  int i0,i1,j0,j1;
  double mag, dphi_di, dphi_dj, del_x, del_y;

  j0 = (int) ((x-MIN_X_DEV)/XDELTA_DEV);
  j1 = (j0+1);
  i1 = NBINS - (int) ((y - MIN_Y_DEV)/YDELTA_DEV); // (int) ((MAX_Y - y)/YDELTA);?
  i0 = (i1-1);

  del_x = (x-MIN_X_DEV)/XDELTA_DEV - j0;
  del_y = (NBINS - (y - MIN_Y_DEV)/YDELTA_DEV) - i0;

  dphi_dj = ((1.0-del_y)*(roger->world_map.potential_map[i0][j1]
			  - roger->world_map.potential_map[i0][j0] ) +
	     (del_y)*(roger->world_map.potential_map[i1][j1]
		      - roger->world_map.potential_map[i1][j0]  ) );
  dphi_di = ((1.0-del_x)*(roger->world_map.potential_map[i1][j0]
			  - roger->world_map.potential_map[i0][j0] ) +
	     (del_x)*(roger->world_map.potential_map[i1][j1]
		      - roger->world_map.potential_map[i0][j1]  ) );

  grad[0] = dphi_dj; grad[1] = -dphi_di;

  mag = sqrt(SQR(grad[0])+SQR(grad[1]));

  if (mag>THRESHOLD) {
    grad[0] /= mag; grad[1] /= mag;
  }
  else grad[0] = grad[1] = 0;
  return(mag);
}

void follow_path(roger)
Robot *roger;
{
  int xbin, ybin;
  double x, y, bb, mag, grad[2], compute_gradient();
  double next_stepsize, velocity_control();

  x = roger->base_position[X];
  y = roger->base_position[Y];

  ybin = (int)((MAX_Y_DEV - y)/YDELTA_DEV);
  xbin = (int)((x - MIN_X_DEV)/XDELTA_DEV);

  grad[X] = grad[Y] = 0.0;
  if (roger->world_map.occupancy_map[ybin][xbin]!=GOAL) {
    mag = compute_gradient(x, y, roger, grad);
    roger->base_setpoint[THETA] = atan2(-grad[Y], -grad[X]);
    roger->base_setpoint[X] = x - PATH_STEPSIZE*grad[X];
    roger->base_setpoint[Y] = y - PATH_STEPSIZE*grad[Y];
  }
  else {
    roger->base_setpoint[X] = roger->base_position[X];
    roger->base_setpoint[Y] = roger->base_position[Y];
  }
}

/************************************************************************/
/************************************************************************/

void sor(), follow_path();

//void updateControlStep(double new_setpoint){
//  CONTROL_STEP = new_setpoint;
//}
//
//double* get_current_goal(){
//  double* goal;
//  return goal;
//}

void project8_control(roger, time)
Robot *roger;
double time;
{
  dilate_obstacles(roger, 2*R_BASE);
  sor(roger);

  follow_path(roger);
}

/************************************************************************/
void project8_reset(roger)
Robot* roger;
{ }

// prompt for and read user customized input values
void project8_enter_params()
{
  printf("Project 8 enter_params called. \n");
}

//function called when the 'visualize' button on the gui is pressed
void project8_visualize(roger)
Robot* roger;
{
  void draw_streamline();
  draw_streamline(roger);
}