#include <SoftTimer.h>

int trigPin = 14;  // A0
int echoPin = 15;  // A1

byte anode[4]  =  {4,    3,    2,    5};
                // Dig1, Dig2, Dig3, Dig4     

byte cathode[7] = {7, 8, 10,12,13,6, 9};
                // A, B, C, D, E, F, G  
                
byte four_digits[4][4] = {
  {1, 0, 0, 0}, // First  digit is on
  {0, 1, 0, 0}, // Second digit is on  
  {0, 0, 1, 0}, // Third  digit is on
  {0, 0, 0, 1}  // Fourth digit is on
};
byte seven_segments[14][7] = {
  { 0,0,0,0,0,0,1 },  // = 0
  { 1,0,0,1,1,1,1 },  // = 1
  { 0,0,1,0,0,1,0 },  // = 2
  { 0,0,0,0,1,1,0 },  // = 3
  { 1,0,0,1,1,0,0 },  // = 4
  { 0,1,0,0,1,0,0 },  // = 5
  { 0,1,0,0,0,0,0 },  // = 6
  { 0,0,0,1,1,1,1 },  // = 7
  { 0,0,0,0,0,0,0 },  // = 8
  { 0,0,0,0,1,0,0 },  // = 9
  { 1,1,1,1,1,1,1 },  // = 10 (OFF)
  { 1,1,1,1,1,1,0 },  // = 11 ("-")
  { 0,1,1,0,0,0,0 },  // = 12 ("E")
  { 1,1,1,1,0,1,0 }   // = 13 ("r")  
}; 

int delDisplayTime = 5;

int  ones, tens, hundreds, thousands;
long duration; 
int  distance;
int offset = 0;


const int numReadings = 5;
int readings[numReadings];      // the readings from the analog input
int index = 0;                  // the index of the current reading
int total = 0;                  // the running total
int average = 0;                // the average

Task TaskDistanceMeasure(50, DistanceMeasure);
Task TaskLED_Redraw(1, LED_Redraw);

void setup() {
  SoftTimer.add(&TaskDistanceMeasure);
  SoftTimer.add(&TaskLED_Redraw);
  
  for (byte a = 0; a < 4; ++a) {
    pinMode (anode[a], OUTPUT);
  }
  for (byte b = 0; b < 7; ++b) {
    pinMode (cathode[b], OUTPUT);
  }
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  for (int thisReading = 0; thisReading < numReadings; thisReading++){
    readings[thisReading] = 0;
  }
    
}

void DistanceMeasure(Task* me) {
  
    // subtract the last reading:
    total= total - readings[index];         
    // read from the sensor:  
    readings[index] = Echo(); 
    // add the reading to the total:
    total= total + readings[index];       
    // advance to the next position in the array:  
    index ++;                    
    // if we're at the end of the array...
    if (index >= numReadings) {
      index = 0;                           
      average = total / numReadings;
    }
    distance = average + offset;
}

long Echo(){
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2); 
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long dur = pulseIn(echoPin, HIGH);
    long dist = (dur/2) / 29.1;
    return dist;
}

void LED_oneDigitWrite(byte digit, byte value) {
    for (byte digCount = 0; digCount < 4; ++digCount) {
      digitalWrite(anode[digCount], four_digits[digit-1][digCount]);
    }
    for (byte segCount = 0; segCount < 7; ++segCount) {
      digitalWrite(cathode[segCount], seven_segments[value][segCount]);
    }
    delay(delDisplayTime);
}

void LED_WriteNumber(int number) {
    ones = (number%10);
    tens = ((number/10)%10);
    hundreds = ((number/100)%10);

    if (hundreds ==0) hundreds = 10;
    if (tens ==0) tens = 10;

    LED_oneDigitWrite(1, hundreds);
    LED_oneDigitWrite(2, 10);
    LED_oneDigitWrite(3, tens);  
    LED_oneDigitWrite(4, ones);
}    

void LED_WriteError(){
    LED_oneDigitWrite(1, 12);
    LED_oneDigitWrite(2, 13);
    LED_oneDigitWrite(3, 13);  
    LED_oneDigitWrite(4, 10);
    delay(delDisplayTime);    
}

void LED_Redraw(Task* me) {
  if (distance >= 300 || distance < 0) LED_WriteError();
  else LED_WriteNumber(distance);  
}