//
// Cecil
//
// Cecil has a sonar unit that can look ahead and determine the distance to an object.
// If an object appears in front of him, he leans towards it, as if curious.
// If the object gets closer than about a foot, it moves backwards, as if to keep some personal space.
//


#include <Servo.h> 

Servo head;

enum { TRIGGERPIN = P2_2, ECHOPIN = P2_1 };
enum { START_ANGLE = 90, MAX_ANGLE=150, MIN_ANGLE=20, NEAR=30, FAR=80 };
enum { FORWARD_STEP=2, BACK_STEP=2 };
enum { NUM_VALUES=8, NOISE=2 };

unsigned long last_time = 0;
enum { WAIT_TIME=8 };

unsigned int
get_distance( void )
{
  unsigned long int now = millis();
  if( now < last_time + WAIT_TIME )
    delay( WAIT_TIME - (now - last_time) );

  digitalWrite( TRIGGERPIN, LOW );
  delayMicroseconds( 2 );
  digitalWrite( TRIGGERPIN, HIGH );
  delayMicroseconds( 10 );
  digitalWrite( TRIGGERPIN, LOW );
  
  unsigned int duration = pulseIn( ECHOPIN, HIGH, 38000 );    // 38 milliseconds if no obstacle
  
  unsigned int distance = duration / 58.77;
  if( distance < 0 )
    distance = 0;
  if( distance > 500 )                                        // The HC-SR04 has a 5 meter range
    distance = 500;
    
  last_time = millis();

  return distance;
}

int distances[ NUM_VALUES ];

void
sort( int *list, int left, int right )
{  
  int i = left;
  int j = right;
  int pivot = list[ (left + right) / 2 ];
 
  while( i <= j )
  {
    while( list[ i ] < pivot )
      i++;
    while( list[ j ] > pivot )
      j--;
    if( i <= j )
    {
      unsigned int tmp = list[ i ];
      list[ i++ ] = list[ j ];
      list[ j-- ] = tmp;
    }
  }
  
  if( left < j )
    sort( list, left, j );
  if( i < right )
    sort( list, i, right );
}

unsigned int
avg_distance( void )
{ 
  for( unsigned int x = 0; x < NUM_VALUES; x++ )
    distances[ x ] = get_distance();
  
  sort( distances, 0, NUM_VALUES-1 );

  unsigned int dist = 0;
  for( unsigned int x = NOISE; x < NUM_VALUES-NOISE; x++ )
    dist += distances[ x ];    
  dist /= NUM_VALUES - (2*NOISE);
  
  return dist;
}

unsigned int angle = START_ANGLE;

void
setup( void )
{
  delay( 5000 );
  Serial.begin( 9600 );
  delay( 1000 );
  Serial.println( "Cecil starting up" );
  P1DIR = 0b11111110;
  P1OUT = 0;
  P2DIR = 0b1101;
  P2OUT = 0b1101;          // P2_0 is servo output, P2_1 is echo input, P2_2 is ping trigger, P2_3 is the speaker
     
  head.attach( P2_0 );  
  head.write( angle );
}

void
back_up( void )
{
  if( angle < MAX_ANGLE )
    angle += BACK_STEP;
  else
    return;
  Serial.print( "Back up" );
  head.write( angle );
}


void
straighten_up( void )
{
  if( angle > START_ANGLE )
  {
    angle--;
    Serial.print( "Straighten up forward (angle = " );
    Serial.print( angle );
    Serial.print( " )" );
    head.write( angle );
  }
  else if( angle < START_ANGLE )
  {
    angle++;
    Serial.print( "Straighten up backward (angle = " );
    Serial.print( angle );
    Serial.print( " )" );
    head.write( angle );
  }
}

void
lean_in( void )
{
  if( angle > MIN_ANGLE )
    angle -= FORWARD_STEP;
  else
    return;
  Serial.print( "Lean in" );
  head.write( angle );
}

void
loop( void )
{
  unsigned int dist = avg_distance();
  Serial.print( "Distance is " );
  Serial.print( dist );
  Serial.print( "   " );
  
  if( dist > NEAR && dist < FAR )
  {
    lean_in();
  }
  else if( dist > FAR )
  {
    straighten_up();
  }
  else if( dist > 0 && dist < NEAR )
  {
    back_up();
  }
  Serial.println( "" );
}