/* 
   Solution to LSU EE 4770 Spring 1999 Real Time Systems Homework 3

   Compiles on Sun Solaris with gcc and cc using math and Posix libraries.

     gcc hw03.c -gstabs -o hw03 -lposix4 -lm -Wall
   or
     cc hw03.c -g -o hw03 -lposix4 -lm 

 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
long random(void);  /* This should be in the include file but isn't. */

#define MAX_IRRAD   10      /* milliwatts / cm^2			      */
#define ARM_LEN	    25	    /* cm					      */
#define REV_TIME    10	    /* seconds. Time for arm to make a revolution.    */
#define PHOTO_PREC (1<<18)  /* Precision of photodetector ADC.		      */
#define ANGLE_PREC (1<<16)  /* Precision of angle counter.		      */

#define PHOTODETECTOR 1
#define ARMCOUNTER 2

/* Declarations for sensor simulation.					      */
double sim_x         = 63;    /* cm					      */
double sim_y	     = 156;   /* cm					      */
#define SIM_RAD_FLUX   10000  /* Light milliWatts			      */
int revs	     = 5;     /* Number of revolutions to run simulation for. */
double sim_arm_angle = 0;
double sim_distance;	      /* Set by read interface.			      */


int readInterface(int port)
{
  if( port == PHOTODETECTOR ) {
    double delta_x = sim_x - ARM_LEN * sin(sim_arm_angle);
    double delta_y = sim_y - ARM_LEN * cos(sim_arm_angle);
    double distance_sq = delta_x * delta_x + delta_y * delta_y;
    double irrad =
      SIM_RAD_FLUX / ( 4.0 * M_PI * distance_sq );
    double adc_out = irrad / MAX_IRRAD * PHOTO_PREC + ( random() % 3 ) - 1;
    sim_distance = pow( distance_sq, 0.5 );
    return adc_out >= PHOTO_PREC ? PHOTO_PREC-1 : adc_out;
  } else {
    return sim_arm_angle / ( 2.0 * M_PI ) * ANGLE_PREC;
  }
}

void sleep_until(double time)
{
  static double last_time = 0;

  if( last_time == 0 ) {  /* Inititalize */
    sim_arm_angle = random() * 2.0 * M_PI / RAND_MAX;
  } else {
    sim_arm_angle += 2.0 * M_PI * (time-last_time) / REV_TIME;
    while( sim_arm_angle >= 2.0 * M_PI ) sim_arm_angle -= 2.0 * M_PI;
  }
  last_time = time;
}

double get_current_time()
{
  struct timespec tp; 
  /* clock_gettime is part of posix library. Can substitute time or, since
     the return value isn't really used, can comment out completely. */
  clock_gettime(CLOCK_REALTIME, &tp);  
  return ((double)tp.tv_sec) + tp.tv_nsec / 1e9; 
  /*  return 0.0;  */
}

double light_direction, light_distance, light_radiant_flux;
void see_the_light()
{
  double next_sample_time = get_current_time(); /* Time in seconds since 1970 */
  int min_photo_raw = 1<<17;
  int max_photo_raw = -1;
  double light_direction = 0;
  double next_direction = 0;

  /* Main Loop */
  while(revs){
    int photo_raw = readInterface(PHOTODETECTOR);
    double arm_angle = readInterface(ARMCOUNTER) * 360.0 / ANGLE_PREC;
    double phase = light_direction - arm_angle;
    if( phase < 0 ) phase += 360;

    if( phase < 90 || phase > 270 ) {
      if( min_photo_raw < PHOTO_PREC ) {
	double ratio = ((double)max_photo_raw) / min_photo_raw;
	double ratio_maybe = ((double)max_photo_raw-1) / (min_photo_raw+1);
	double ratio_or_maybe = ((double)max_photo_raw+1) / (min_photo_raw-1);
	double max_irrad = ((double)max_photo_raw) * MAX_IRRAD / PHOTO_PREC;
	double light_distance_maybe = ARM_LEN +
	  2.0 * ARM_LEN * ( 1.0 + pow(ratio_maybe,0.5) )
	  / ( ratio_maybe - 1.0 );
	double light_distance_or_maybe = ARM_LEN +
	  2.0 * ARM_LEN * ( 1.0 + pow(ratio_or_maybe,0.5) )
	  / ( ratio_or_maybe - 1.0 );
	double error_maybe = light_distance_maybe - light_distance;
	double error_or_maybe = light_distance - light_distance_or_maybe;
	double the_error =
	  error_maybe > error_or_maybe ? error_maybe : error_or_maybe;
	  light_direction = next_direction;
	light_distance = ARM_LEN +
	  2.0 * ARM_LEN * ( 1.0 + pow(ratio,0.5) ) / ( ratio - 1.0 );
	light_radiant_flux =
	  4.0 * M_PI * light_distance * light_distance * max_irrad;
	printf("(%3d,%3d) Dist, %f; angle, %6.2f deg; radiant flux, %f mW\n",
	       min_photo_raw, max_photo_raw,
	       light_distance, light_direction, light_radiant_flux);
	printf("Precision %f\n", the_error);
	min_photo_raw = PHOTO_PREC;
	max_photo_raw = 0;
	revs--;
      } 
      if( photo_raw > max_photo_raw ) {
	max_photo_raw = photo_raw;
	next_direction = arm_angle;
      }
    } else {
      if( photo_raw < min_photo_raw ) {
	min_photo_raw = photo_raw;
      }
    }

    next_sample_time += 10.0 / ANGLE_PREC;
    sleep_until(next_sample_time);
  }
}

int main(int argv, char **argc)
{
  double angle = atan2(sim_x,sim_y) / ( 2.0 * M_PI ) * 360.0;
  if( angle < 0 ) angle += 180;
  printf("Initial position %5.1f, %5.1f\n",sim_x,sim_y);
  printf("Simulated distance, %f cm; angle %6.2f deg; rf %d mW\n",
	 pow( sim_x * sim_x + sim_y * sim_y, 0.5), angle, SIM_RAD_FLUX);
	 
  see_the_light();
  return 0;
}