/*
    test PSD by  Kimio Kosaka
*/

#include <LiquidCrystal.h>

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

void setup()
{
  lcd.begin(16, 2); // set up the LCD's number of rows and columns:
  lcd.clear(); // clear LCD
  lcd.print("Distance");
}

#define PSD 0    //PSD connect Analog port 0
#define WAIT 2   //wait time 2mS

void loop()
{
  unsigned int distance;
  distance=getpsd(PSD,WAIT); //get distance
  lcd.setCursor(0,1);
  lcd.print(distance);
  lcd.print("cm    ");
}


int compare_int(const void* a, const void* b)
{
	return ( *(int*)a - *(int*)b );
}

unsigned int  getpsd(byte port,byte rd) // return value [mm]
{
  #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
  #define N 21
  #define M 11
  double y;
  unsigned int buff[N],n;
  for(n=0;n<N;n++){
    buff[n]=analogRead(port);
    delay(rd);
  }
  qsort( buff, ARRAY_SIZE(buff), sizeof(int), compare_int );
  n=(buff[M-2]+buff[M-1]+buff[M]+buff[M+1]+buff[M+2])/5;
  y=(6787.0 / (n - 3.0)) - 4.0;// by Making Things Talk
  return (unsigned int)y;
}