// ClassifierDemo.ino : Arduino program to demonstrate application of a decision tree.
// No copyright, 2020, Garth Zeglin.  This file is explicitly placed in the public domain.

// The decision tree function and is kept in a separate .ino files which will
// automatically be compiled with this one by the Arduino IDE.  The tree code
// was generated from data using classify_gen.py.

// The baud rate is the number of bits per second transmitted over the serial port.
const long BAUD_RATE = 115200;

//================================================================
// Hardware definitions. You will need to customize this for your specific hardware.
const int sonarTriggerPin = 7;    // Specify a pin for a sonar trigger output.
const int sonarEchoPin    = 8;    // Specify a pin for a sonar echo input.

//================================================================
// Standard Arduino initialization function to configure the system.
void setup()
{
  // initialize the Serial port
  Serial.begin( BAUD_RATE );

  // Initialize the digital input/output pins.
  pinMode(sonarTriggerPin, OUTPUT);
  pinMode(sonarEchoPin, INPUT);
}

//================================================================
// Standard Arduino polling function. This function is called repeatedly to
// handle all I/O and periodic processing.  This loop should never be allowed to
// stall or block so that all tasks can be constantly serviced.

void loop()
{
  // Calculate the interval in microseconds since the last polling cycle.
  static unsigned long last_time = 0;
  unsigned long now = micros();
  unsigned long interval = now - last_time;
  last_time = now;

  // Poll the sonar at regular intervals.
  static long sonar_timer = 0;
  sonar_timer -= interval;
  if (sonar_timer < 0) {
    sonar_timer += 100000; // 10 Hz sampling rate

    // read the sonar; zeros represent a no-ping condition
    int raw_ping = ping_sonar();

    // suppress zeros in the input, just repeating the last input
    int nz_ping = suppress_value(raw_ping, 0);

    // convert the value from microseconds to centimeters
    float cm = fmap(nz_ping, 0.0, 5900.0, 0.0, 100.0);

    // apply a low-pass filter to smooth the raw data
    cm = lowpass(cm);
    
    // fit a trajectory curve to recent sample history
    float traj[3];
    trajfit(cm, traj);

    // quantize and classify the current estimation
    int posvel[2];
    posvel[0] = (int) traj[0];
    posvel[1] = (int) traj[1];
    int cls = classify(posvel);

    // debounce the classification to eliminate transient changes
    cls = debounce(cls, 5);

    // emit some data to plot
    // Serial.print(raw_ping); Serial.print(" ");      // ping time in microseconds
    // Serial.print(cm); Serial.print("  ");           // centimeter-scaled, zero-suppressed
    // Serial.print(traj[0]); Serial.print(" ");       // quadratic position
    // Serial.print(traj[1]); Serial.print(" ");       // quadratic velocity
    Serial.print(posvel[0]); Serial.print(",");    // integer position for classification
    Serial.print(posvel[1]); Serial.print(" ");    // integer velocity for classification
    Serial.print(20*cls); Serial.print(" ");       // integer sample classification, amplified for live plotting
    
    Serial.println();
  }
}