#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 */
double sim_distance; /* Set by read interface. */
int revs = 5;  /* Number of revolutions to run simulation for. */

double sim_arm_angle = 0;
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;
  }
  return -1; /* Avoid compiler warnings. */
}

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 */
  /* Insert solution here. */

  /* Main Loop */
  while(revs){
    int photo_raw = readInterface(PHOTODETECTOR);
    double arm_angle_raw = readInterface(ARMCOUNTER);
    /* Insert solution here. */

    light_direction = 0 /* Replace.. */;
    light_distance = 0 /* Replace.. */; 
    light_radiant_flux = 0 /* Replace.. */;


    next_sample_time = 0 /* Replace.. */;
    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;
}