#include <Plaquette.h>

const int N_CHANNELS = 4;

// Multiplexer channel selection signals.
DigitalOut sOut[4] = {2, 3, 4, 5};

// Channel values.
float values[N_CHANNELS];

// Common input to read value.
AnalogIn commonInput(A0);

// Square wave used to trigger multiplexer, 
// wait and then trigger common input read.
SquareWave switcher(0.1);

// Current channel for the switching.
int currentChannel = 0;

void begin()
{
  // Initialize.
  for (int i=0; i<N_CHANNELS; i++) {
    sOut[i].off();
    values[i] = 0;
  }
}

void step() {
  if (switcher.rose()) {
    // Set channel on multiplexer.
    mux(currentChannel);  
  }
  else if (switcher.fell()) {
    // Save channel value.
    values[currentChannel] = commonInput;

    // Update channel.
    currentChannel++;

    // All channels have been checked: print and reset.
    if (currentChannel >= N_CHANNELS) {
      printValues();
      currentChannel = 0;
    }
  }
}

// Print values for display on the plotter.
void printValues() {
  for (int i=0; i<N_CHANNELS; i++) {
    print(values[i]); print(" ");
  }
  println(); // newline
}

// Set multiplexer to send value from channel.
void mux(int channel) {
  for (int i=0; i<N_CHANNELS; i++) {
    bitRead(channel, i) >> sOut[i];
  }
}