//
// FILE: sonar.cpp
// programmer: Dave Hershberger
// purpose:    sonar interface object for Clem.
// date: 1-9-96

#include <stdio.h>
#include <stdlib.h>

#include "comp_dep.h"
#include "timer.h"
#include "sonar.h"
#include "hardware.h"
#include "mrv4ms.h"

Sonar *Sonar::me = NULL;
int Sonar::new_values_available;
float Sonar::latest_values[NUM_ULTRASONICS];
int Sonar::pattern_index;
int Sonar::firing_on;
long int Sonar::pattern[8];

// the global sonar pointer is declared in sonar.h and defined here.
//Sonar * const sonar = new Sonar;

Sonar::Sonar()
{
  if(!me){
    me = this;

    new_values_available = 0;
    pattern_index = 0;
    firing_on = 0;
    pattern[0] = 0x010101;
    pattern[1] = 0x080808;
    pattern[2] = 0x404040;
    pattern[3] = 0x020202;
    pattern[4] = 0x101010;
    pattern[5] = 0x808080;
    pattern[6] = 0x040404;
    pattern[7] = 0x202020;
  }
}

#define SIM (int)179
#define SID (int)256

void sonar_timer_callback(double elapsed_time)
{
  const float scaleFact=0.1;
  static int i;

  if(Sonar::me->firing_on){
    Sonar::me->pattern_index++;
    if(Sonar::me->pattern_index >= 8){
      Sonar::me->pattern_index = 0;

      // Is there a problem with this? sonartst.cpp shows strange
      // behavior on sonars 0, 8, and 16. Hmmm. -Dave

      for(i = 0; i < NUM_ULTRASONICS; i++){
	Sonar::me->latest_values[i] = (float) ((((int) sonar_dist[i + 1])
					    * SIM / SID) & 255) * scaleFact;
	sonar_dist[i + 1] = 255;
      }

      Sonar::me->new_values_available = 1;
    }
    fire_sonars_pattern(Sonar::me->pattern[Sonar::me->pattern_index]);
  }
}

void Sonar::start()
{
  if(!firing_on){

    Sonars_Initialise();
    
    pattern_index = 0;
    timer->add_callback(sonar_timer_callback, TICKS_PER_SONAR_FIRE, 0);
    
    firing_on = 1;
  }
}

void Sonar::read(float *data)
{
  int i;
  
  for (i = 0; i < NUM_ULTRASONICS; i++)
    data[i] = latest_values[i];
  
  new_values_available = 0;
}

void Sonar::stop()
{
  if(firing_on){
    firing_on = 0;
    
    timer->delete_callback(sonar_timer_callback, TICKS_PER_SONAR_FIRE);
    Sonars_Finalise();
  }
}