//#include <FreqCounter.h>
#include "Arduino.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <FlexiTimer2.h>

// Actual (accurate) measurements
// 132322 Hz = off
// 141328 Hz = on

#define DISCONNECTED_FREQUENCY 10000
#define THRESHOLD_FREQUENCY 52500

#define POWER_RELAY 15
#define SPEAKER_RELAY 16
#define LED 13
#define OPTICAL_POWER 4

// Prescaler factors for high and low speed.
// mode 0 - Do nothing - normal 16Mhz clock
// mode 1 - Clock divide by 2 - 8Mhz
// Mode 2 - Clock divide by 4 - 4Mhz
// Mode 3 - Clock divide by 8 - 2Mhz
// Mode 4 - Clock divide by 16 - 1Mhz
#define PRESCALER_LOW 4
#define PRESCALER_HIGH 1

#define DO_SERIAL
//#define OVERFLOWS

// I found this at ftp://82.140.81.3/pub/atmel/_AVR_MCUs_8bit/_WITH_USB/AT90USBKEY_CD/starterkits/STK525-USBKEY/firmware/at90usb128-usbkey-demo-3enum-host-mouse-1_0_4/Atmel/at90usb128-usbkey-demo-3enum-host-mouse/at90usb128/demo/ms_mouse_host_mouse/doc/html/a00101.html
#define 	Set_prescaler(x)   (CLKPR = (1<<CLKPCE),CLKPR = x)

// These are volatile because they're used in the Timer0 ISR and the main (timer2) ISR
volatile unsigned long frequency_counter = 0;
volatile unsigned int short_tick_counter = 1;
#ifdef OVERFLOWS
volatile unsigned int overflows = 0;
#endif

// THese are only used inside the isr() function, so don't need to be volatile
unsigned int long_tick_counter = 0;
int state = 0;
char state_direction = 0;
unsigned char taking_reading = 0;

// Timer0 ISR. Notionally this gets called about once a millisecond. In practice it's
// no where near that. Also, it gets disabled a lot of the time, as it's only used when
// we're actually taking a reading. When we are taking a reading, we run at high clock
// speed, and the rest of the time were at super-low speed.
ISR(TIMER0_COMPA_vect) {
  frequency_counter = frequency_counter + TCNT1;
  TCNT1=0;
#ifdef OVERFLOWS
  if (TIFR1 & 1) {
    overflows++;
    TIFR1 =(1<<TOV1);
  }
#endif
  short_tick_counter++;
}


// This is the main Timer2 ISR. Notionally it runs every 100ms, although in practice it's
// nothing like that. It runs all the time that the chip is powered, and takes care of
// all of the frequency reading and relay control.
void isr(void) {
  long_tick_counter++;
  
  if(taking_reading) {
    // stop taking frequency reading...
    TIMSK0 &=~(1<<OCIE0A);       // disable Timer0  //disable  millis and delay
    digitalWrite(LED, LOW);
    Set_prescaler(PRESCALER_LOW);

#ifdef DO_SERIAL
    Serial.print(short_tick_counter);
    Serial.print(" ");
#ifdef OVERFLOWS
    Serial.print(overflows);
    Serial.print(" ");
#endif
    Serial.println(frequency_counter);
#endif
    
#ifdef OVERFLOWS
    if(overflows) {
      frequency_counter = (overflows * 65535) + frequency_counter;
    }
#endif
    
    taking_reading = 0;
    
    digitalWrite(OPTICAL_POWER,LOW);
    
    if(frequency_counter < DISCONNECTED_FREQUENCY || frequency_counter > THRESHOLD_FREQUENCY) {
      if(state_direction != 1) {
        state = 0;
        state_direction = 1;
      }
    } else {
      // Frequency is in the "off" zone
      if(state_direction != -1) {
        state = 0;
        state_direction = -1;
      }
    }
  }
  
  if(long_tick_counter == 1) {
    // Take a frequency reading...
    digitalWrite(OPTICAL_POWER,HIGH);
    frequency_counter = 0;
    short_tick_counter = 1;
#ifdef OVERFLOWS
    overflows = 0;
    TIFR1 =(1<<TOV1);    // clear overflow
#endif
    TCNT1=0;
    TCNT0=0;
    
    Set_prescaler(PRESCALER_HIGH);
                                // External clock source on T1 pin. Clock on rising edge.
    TCCR1B |= (1<<CS12) | (1<<CS11) | (1<<CS10);        //   start counting now
    TIMSK0 = _BV(OCIE0A);        // (re)enable timer0
    digitalWrite(LED, HIGH);
    taking_reading = 1;
  } else if(long_tick_counter >=3) {
    long_tick_counter = 0;
  }

  state = state + state_direction;

  if(state == 1) {
    // Turn on the power relay
    digitalWrite(POWER_RELAY, HIGH);
  } else if(state == 5) {
    // turn on the speaker relay
    digitalWrite(SPEAKER_RELAY, HIGH);
    state_direction = 0;
  }
  
  if(state == -20) {
    // Turn off the speaker relay
    digitalWrite(SPEAKER_RELAY, LOW);
  } else if(state == -23) {
    // Turn off the power relay
    digitalWrite(POWER_RELAY, LOW);
    state_direction = 0;
  }
}

void setup(void) {
  int i;
  for(i=0; i <= 19; i++) {
    // Set all pins as inputs, with internal pull-ups
    // This is very nearly as power efficient as setting them all
    // as output/low, but safer in terms of accidental short circuit
    pinMode(i, INPUT);
    digitalWrite(i, HIGH);
  }
  
  // Now setup the pins we're using...
  // pin 5 = the digital input used by the frequency counter
  digitalWrite(5, LOW);    // don't use pull-up on the optical in
  pinMode(POWER_RELAY, OUTPUT);
  pinMode(SPEAKER_RELAY, OUTPUT);
  pinMode(LED, OUTPUT);
  pinMode(OPTICAL_POWER,OUTPUT);
  digitalWrite(POWER_RELAY, LOW);
  digitalWrite(SPEAKER_RELAY, LOW);
  digitalWrite(OPTICAL_POWER,LOW);

#ifdef DO_SERIAL
  // To use the serial port, the receiver's speed needs to be the speed
  // here divided by the PRESCALER_LOW divider. For example:
  // Prescaler mode 4 -> divider = 16, so actual port speed = 2400
  Serial.begin(38400);        // connect to the serial port
#endif  
  
  // hardware counter setup ( refer atmega168.pdf chapter 16-bit counter1)
    TCCR1A=0;                  // reset timer/counter1 control register A
    TCCR1B=0;              	   // reset timer/counter1 control register A
    TCNT1=0;           		   // counter value = 0
    // set timer/counter1 hardware as counter , counts events on pin T1 ( arduino pin 5)
    // normal mode, wgm10 .. wgm13 = 0
    
    TCCR1B |=  (1<<CS10) ;// External clock source on T1 pin. Clock on rising edge.
    TCCR1B |=  (1<<CS11) ;
    TCCR1B |=  (1<<CS12) ;
  
  // Use timer2 to call our ISR every X ms * clock pre-scaler. Since we change
  // between fast as slow clock speeds, this time isnt contant, and so a bit of
  // fiddline here may be required. 
  FlexiTimer2::set(20, isr);
  FlexiTimer2::start();
  
  // Set up TImer 0 to interrupt every millisecond
  //TCCR0B = 0b11;  //Timer settings for interrupt at every millisecond
  OCR0A = 249;
  
  //TIMSK0 |= 0b10;
  //TIMSK0 = _BV(OCIE0A);
  TIMSK0 &=~(1<<OCIE0A);       // disable Timer0  //disable  millis and delay
  
  // Now do some power saving
  // Power reduction register
  // Bit 7 = Two Wire Interface (TWI)
  // Bit 6 = Timer/counter 2
  // Bit 5 = Timer/counter 0
  // Bit 4 = reserved
  // Bit 3 = Timer/counter 1
  // Bit 2 = Serial peripheral interface (SPI)
  // Bit 1 = USART0
  // Bit 0 = ADC
  
  // Turn off TWI, SPI, UART and ADC
#ifndef DO_SERIAL
  PRR = B10000111;
#endif
  
  ADCSRA |= (0<<ADEN);                     // disable ADC (do we need to do this?)
  
  // Reduce our clock speed by a half
  // mode 0 - Do nothing - normal 16Mhz clock
  // mode 1 - Clock divide by 2 - 8Mhz
  // Mode 2 - Clock divide by 4 - 4Mhz
  // Mode 3 - Clock divide by 8 - 2Mhz
  // Mode 4 - Clock divide by 16 - 1Mhz
  Set_prescaler(PRESCALER_LOW);
  
  // Prescale timer 0 to run 8x it's normal speed. This is a bit faster
  // than we want, but it's the best resolution we have. For some reason
  // we can't successfully set the main clock prescaler to mode 3
  // Setting    	Divisor 	Frequency
  // 0x01 	 	1 	 	62500
  // 0x02  		8 	 	7812.5
  // 0x03  		64 	 	976.5625
  // 0x04 	 	256 	 	244.140625
  // 0x05 	 	1024 	 	61.03515625
  TCCR0B = TCCR0B & 0b11111000 | 0x03;
  
  /*
  * The 5 different modes are:
     *     SLEEP_MODE_IDLE         -the least power savings
     *     SLEEP_MODE_ADC
     *     SLEEP_MODE_PWR_SAVE
     *     SLEEP_MODE_STANDBY
     *     SLEEP_MODE_PWR_DOWN     -the most power savings
     *
     * For now, we want as much power savings as possible, so we
     * choose the according
     * sleep mode: SLEEP_MODE_PWR_DOWN
     *
     */  
    set_sleep_mode(SLEEP_MODE_IDLE);   // sleep mode is set here
 
    sleep_enable();          // enables the sleep bit in the mcucr register
                             // so sleep is possible. just a safety pin
 
  
}


// loop() - do as little as possible here! The idea is that interrupts do abolutely
// everything, so we can go to sleep in between them running.
void loop(void) {
  sleep_mode();            // here the device is actually put to sleep!!
}