#include <TimerOne.h>
#include <MultiFuncShield.h>
#include <Wire.h>

/* Einstellungsmögichkeiten  */
#define INTERVAL_NORMAL_MODE 2
#define INTERVAL_SCAN_MODE 1
#define INTERVAL_TIME_SCAN_MODE 3
#define STARTVALUE_FREQUENCY 40


/* Einstellungsmögichkeiten ENDE */

#define FREQ_PIN 9
#define FREQ(kHz) (1000/(kHz))
const int freq[] = { 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60 };
const int freq_size = (sizeof(freq) / sizeof(int));
uint16_t index = STARTVALUE_FREQUENCY - 5;

void setup() {
  Serial.begin(9600);
  MFS.initialize(&Timer1);
  Timer1.initialize(FREQ(freq[index]));         // initialize timer1, and set start frequency
  Timer1.pwm(FREQ_PIN, 512);                // setup pwm on pin 9, 50% duty cycle
  MFS.write(freq[index]);                   //write frequency to display
}

void scan()
{
  MFS.write("SCAN");
  delay(1000);
  bool endLoop = false;
  index = 0;
  while ( endLoop == false and index < freq_size)
  {
    Timer1.initialize(FREQ(freq[index]));         // initialize timer1, and set a 1/2 second period
    Timer1.pwm(FREQ_PIN, 512);                // setup pwm on pin 9, 50% duty cycle
    MFS.write(freq[index]);
    for (int i = 0; i < (200 * INTERVAL_TIME_SCAN_MODE)  and endLoop == false; ++i)
    {
      byte btn = MFS.getButton();
      if (btn)
      {
        byte buttonNumber = btn & B00111111;
        byte buttonAction = btn & B11000000;

        if (buttonAction == BUTTON_PRESSED_IND)
        {
          if (buttonNumber == 1)
            endLoop = true;
        }
      }
      delay(5);
    }
    index += INTERVAL_SCAN_MODE;
  }
  if(endLoop == false)
      index = STARTVALUE_FREQUENCY - 5;

  Timer1.initialize(FREQ(freq[index]));         // initialize timer1, and set a 1/2 second period
    Timer1.pwm(FREQ_PIN, 512);                // setup pwm on pin 9, 50% duty cycle
    MFS.write(freq[index]);
}


void loop() {
  delay(5);
  // put your main code here, to run repeatedly:
  byte btn = MFS.getButton(); // Normally it is sufficient to compare the return
  // value to predefined macros, e.g. BUTTON_1_PRESSED,
  // BUTTON_1_LONG_PRESSED etc.
  if (btn)
  {
    byte buttonNumber = btn & B00111111;
    byte buttonAction = btn & B11000000;
    ;
    if (buttonAction == BUTTON_PRESSED_IND)
    {

      if (buttonNumber == 2 ) {
        index -= INTERVAL_NORMAL_MODE;
        if (index >= freq_size)
          index = freq_size - 1;
        index %= freq_size;
      }
      if (buttonNumber == 3 )
        index = (index + INTERVAL_NORMAL_MODE ) % freq_size;
      
      Timer1.initialize(FREQ(freq[index]));         // initialize timer1, and set a 1/2 second period
      Timer1.pwm(FREQ_PIN, 512);                // setup pwm on pin 9, 50% duty cycle
      MFS.write(freq[index]);
      if (buttonNumber == 1)
      {
        scan();
      }
    }
  }
}