#include <Arduino.h>
#include "pin_definitions.h"
#include "config.h"

// Function prototypes
void setup(void);
void loop(void);
float mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh);
void calculateOutputInterval(uint8_t fanIndex, float convFactor);
void halfRevolution(byte fanIndex);

volatile unsigned long previousMicros[4] = {0, 0, 0, 0};
volatile float avgInterval[4] = {100.0f, 100.0f, 100.0f, 100.0f};
uint16_t toggleA[4] = {4687, 4687, 4687, 4687};
uint16_t toggleB[4] = {2343, 2343, 2343, 2343};

void setup() {
  Serial.begin(115200);
  
  #ifdef POT_PIN_0
    pinMode(POT_PIN_0, INPUT);
  #endif
  #ifdef POT_PIN_1
    pinMode(POT_PIN_1, INPUT);
  #endif
  #ifdef POT_PIN_2
    pinMode(POT_PIN_2, INPUT);
  #endif
  #ifdef POT_PIN_3
    pinMode(POT_PIN_3, INPUT);
  #endif

  cli();  // disable global interrupts

  #ifdef IN_PIN_0
    // Timer1
    pinMode(IN_PIN_0, INPUT_PULLUP);
    // Connect OUT_PIN_0 to ground
    pinMode(OUT_PIN_0, OUTPUT);
    digitalWrite(OUT_PIN_0, LOW);

    attachInterrupt(digitalPinToInterrupt(IN_PIN_0), []() {halfRevolution(0);}, RISING);

    TCCR1A = 0;  // Disconnect output pins from timer 1
    TCCR1B = (1 << WGM12);  // Set Timer 1 to CTC mode

    OCR1A = toggleA[0];  // Set 16-bit Output Compare Register A (TOP value / High trigger time)
    OCR1B = toggleB[0];  // Set 16-bit Output Compare Register B (Low trigger time)
    TIMSK1 |= (1 << OCIE1A);  // Enable TIMER1_COMPA interrupt
    TIMSK1 |= (1 << OCIE1B);  // Enable TIMER1_COMPB interrupt
    TCCR1B |= PRESCALER;
  #endif

  #ifdef IN_PIN_1
    // Timer3
    pinMode(IN_PIN_1, INPUT_PULLUP);
    pinMode(OUT_PIN_1, OUTPUT); digitalWrite(OUT_PIN_1, LOW);
    attachInterrupt(digitalPinToInterrupt(IN_PIN_1), []() {halfRevolution(1);}, RISING);
    TCCR3A = 0;  // Disconnect output pins from timer 3
    TCCR3B = (1 << WGM32) | PRESCALER;  // Set Timer 3 to CTC mode and add prescaler
    OCR3A = toggleA[1]; OCR3B = toggleB[1];
    TIMSK3 |= (1 << OCIE3A) | (1 << OCIE3B);
  #endif

  #ifdef IN_PIN_2
    // Timer4
    pinMode(IN_PIN_2, INPUT_PULLUP);
    pinMode(OUT_PIN_2, OUTPUT); digitalWrite(OUT_PIN_2, LOW);
    attachInterrupt(digitalPinToInterrupt(IN_PIN_2), []() {halfRevolution(2);}, RISING);
    TCCR4A = 0;  // Disconnect output pins from timer 4
    TCCR4B = (1 << WGM42) | PRESCALER;  // Set Timer 4 to CTC mode and add prescaler
    OCR4A = toggleA[2]; OCR4B = toggleB[2];
    TIMSK4 |= (1 << OCIE4A) | (1 << OCIE4B);
  #endif

  #ifdef IN_PIN_3
    // Timer5
    pinMode(IN_PIN_3, INPUT_PULLUP);
    pinMode(OUT_PIN_3, OUTPUT); digitalWrite(OUT_PIN_3, LOW);
    attachInterrupt(digitalPinToInterrupt(IN_PIN_3), []() {halfRevolution(3);}, RISING);
    TCCR5A = 0;  // Disconnect output pins from timer 5
    TCCR5B = (1 << WGM52) | PRESCALER;  // Set Timer 5 to CTC mode and add prescaler
    OCR5A = toggleA[3]; OCR5B = toggleB[3];
    TIMSK5 |= (1 << OCIE5A) | (1 << OCIE5B);
  #endif

  sei();  // enable global interrupts
}

void loop() {
  float convFactor;
  #ifdef IN_PIN_0
    #ifdef POT_PIN_0
      convFactor = mapFloat(analogRead(POT_PIN_0), 0, 1023, FACTOR_POT_MIN, FACTOR_POT_MAX);
    #else
      convFactor = FACTOR;
    #endif
    calculateOutputInterval(0, convFactor);
    #if defined IN_PIN_1 || defined IN_PIN_2 || defined IN_PIN_3
      Serial.print(",");
    #else
      Serial.print("\n");
    #endif
  #endif

  #ifdef IN_PIN_1
    #ifdef POT_PIN_1
      convFactor = mapFloat(analogRead(POT_PIN_1), 0, 1023, FACTOR_POT_MIN, FACTOR_POT_MAX);
    #else
      convFactor = FACTOR;
    #endif
    calculateOutputInterval(1, convFactor);
    #if defined IN_PIN_2 || defined IN_PIN_3
      Serial.print(",");
    #else
      Serial.print("\n");
    #endif
  #endif

  #ifdef IN_PIN_2
    #ifdef POT_PIN_2
      convFactor = mapFloat(analogRead(POT_PIN_2), 0, 1023, FACTOR_POT_MIN, FACTOR_POT_MAX);
    #else
      convFactor = FACTOR;
    #endif
    calculateOutputInterval(2, convFactor);
    #ifdef IN_PIN_3
      Serial.print(",");
    #else
      Serial.print("\n");
    #endif
  #endif

  #ifdef IN_PIN_3
    #ifdef POT_PIN_3
      convFactor = mapFloat(analogRead(POT_PIN_3), 0, 1023, FACTOR_POT_MIN, FACTOR_POT_MAX);
    #else
      convFactor = FACTOR;
    #endif
    calculateOutputInterval(3, convFactor);
    Serial.print("\n");
  #endif
  delay(UPDATE_INTERVAL_MS);
}

// Convert a float value from one range to another
float mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh) {
  return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow; 
}

void calculateOutputInterval(byte fanIndex, float convFactor) {
    float maxCount;

    maxCount = avgInterval[fanIndex] / (convFactor * US_TO_COUNT);
    maxCount = constrain(maxCount, 2.0f, 65535.0f);

    toggleA[fanIndex] = lround(maxCount);
    toggleB[fanIndex] = toggleA[fanIndex]/2;

    Serial.print("Fan_");
    Serial.print(fanIndex, DEC);
    Serial.print("_in:");
    Serial.print(toggleA[fanIndex], DEC);
    Serial.print(",");
    Serial.print("Fan_");
    Serial.print(fanIndex, DEC);
    Serial.print("_out:");
    Serial.print(toggleB[fanIndex], DEC);
}

// For each rotation of the fan, this interrupt function is triggered twice
void halfRevolution(byte fanIndex) {    
    unsigned long currentMicros = micros();
    // Add some smoothing to the speed
    avgInterval[fanIndex] = (ALPHA*(currentMicros-previousMicros[fanIndex])) + (ONE_MINUS_ALPHA*avgInterval[fanIndex]);

    previousMicros[fanIndex] = currentMicros;
}

#ifdef IN_PIN_0
ISR(TIMER1_COMPA_vect) {
    // Interrupt releasing OUT_PIN_0 (Floating/Letting PC pull HIGH)
    pinMode(OUT_PIN_0, INPUT);
    // OCR1A/B are set here to simulate double buffering that avoids
    // setting TOP lower than the current timer and thus missing the interrupt
    if (OCR1A != toggleA[0]) {
      OCR1A = toggleA[0];
      OCR1B = toggleB[0];
    }
}
ISR(TIMER1_COMPB_vect) {
    // Interrupt connecting OUT_PIN_0 to ground (LOW)
    pinMode(OUT_PIN_0, OUTPUT);
}
#endif

#ifdef IN_PIN_1
ISR(TIMER3_COMPA_vect) {
    pinMode(OUT_PIN_1, INPUT);
    if (OCR3A != toggleA[1]) {
      OCR3A = toggleA[1];
      OCR3B = toggleB[1];
    }
}
ISR(TIMER3_COMPB_vect) {
    pinMode(OUT_PIN_1, OUTPUT);
}
#endif

#ifdef IN_PIN_2
ISR(TIMER4_COMPA_vect) {
    pinMode(OUT_PIN_2, INPUT);
    if (OCR4A != toggleA[2]) {
      OCR4A = toggleA[2];
      OCR4B = toggleB[2];
    }
}
ISR(TIMER4_COMPB_vect) {
    pinMode(OUT_PIN_2, OUTPUT);
}
#endif

#ifdef IN_PIN_3
ISR(TIMER5_COMPA_vect) {
    pinMode(OUT_PIN_3, INPUT);
    if (OCR5A != toggleA[3]) {
      OCR5A = toggleA[3];
      OCR5B = toggleB[3];
    }
}
ISR(TIMER5_COMPB_vect) {
    pinMode(OUT_PIN_3, OUTPUT);
}
#endif