#include <Arduino.h>

static uint8_t led_duration = 4;

/*
 * Function: configureTimer
 * ----------------------------
 *   Configures both 16bit timer for 25 kHz pwm signals.
 */
void configureTimer()
{
    // Configure Timer 1 for PWM @ 25 kHz.
    TCCR1A = 0;   // undo the configuration done by...
    TCCR1B = 0;   // ...the Arduino core library...
    TCCR1C = 0;   // ...for pin 9, 10 and 11.
    TCNT1  = 0;   // reset timer
    // non-inverted PWM on ch. A, B and C, mode 10: ph. correct PWM, TOP = ICR1
    // TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(COM1C1) | _BV(WGM11);

    ICR1   = 640;       // TOP = 320, @16Mhz CPU -> 25kHz PWM
    OCR1A = 500;          // Set duty-cycle
    TCCR1B = _BV(WGM13) | _BV(CS10); // prescaler = 1
    // Configure Timer 3
    // TCCR3A = 0;
    // TCNT3  = 0;
    // TCCR3A = _BV(COM1A1) | _BV(WGM11);
    // TCCR3B = _BV(WGM13) | _BV(CS10);
    // ICR3   = 320;

    // pretty slow ~1000hz
    // TCCR1A = 0;            //clear timer registers
    // TCCR1B = 0;
    // TCNT1 = 0;

    // TCCR1B |= _BV(CS12) | _BV(CS11) | _BV(CS10);   //no prescaler
    // ICR1 = 320;            //PWM mode counts up 320 then down 320 counts (25kHz)

    // TCCR1A |= _BV(COM1A1); //output A clear rising/set falling

    // TCCR1A |= _BV(COM1B1); //output B clear rising/set falling

    // TCCR1B |= _BV(WGM13);  //PWM mode with ICR1 Mode 10
    // TCCR1A |= _BV(WGM11);  //WGM13:WGM10 set 1010
}


/*
 * Function: setup
 * ----------------------------
 *   Default arduino setup function, calls all the initialization functions.
 */
void setup()
{
    Serial.begin(9600);     // initialization of serial over usb.
    delay(500);            // wait a bit for external program to connect.
 
    
    Serial.println("set output pins...");
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);
    pinMode(9, OUTPUT);
    pinMode(7, INPUT);

    // configure timer
    Serial.println("configure timer for 25kHz PWM...");
    configureTimer();

    Serial.println("fan control setup done.");
}

void loop()
{
    uint8_t tacho_last = 0;
    uint8_t tacho_counter = 0;
    uint32_t iterator = 0;
    uint32_t period = 0;
    uint16_t rotations = 0;

    uint16_t led_a_period = (uint16_t)-1;
    uint16_t led_a_counter = led_a_period;

    uint16_t led_b_period = (uint16_t)-1;
    uint16_t led_b_counter = led_b_period;

    while (1) {
        iterator++;

        uint8_t tacho_current = digitalRead(7);

        /* Get number of loop iterations per fan revolution */
        if (!tacho_last && tacho_current) {
            if (tacho_counter == 0) {
                tacho_counter = 1;
            } else {
                tacho_counter = 0;
                period = iterator;
                led_a_period = period / 3;
                led_b_period = period * 20 / 61;
                iterator = 0;
                rotations++;
            }
        }
        tacho_last = tacho_current;

        /* Emit strobe for LED A */
        led_a_counter--;
        if (led_a_counter < led_duration) {
            digitalWrite(2, HIGH);
            if (!led_a_counter) {
                led_a_counter = led_a_period;
            }
        } else {
            digitalWrite(2, LOW);
        }

        /* Emit strobe for LED B */
        led_b_counter--;
        if (led_b_counter < led_duration) {
            digitalWrite(3, HIGH);
            if (!led_b_counter) {
                led_b_counter = led_b_period;
            }
        } else {
            digitalWrite(3, LOW);
        }
    }  
}