#include "UnoPWM.h"
#include "common.h"

// Constructor with frequency and optional PWM pins
UnoPWM::UnoPWM(word pwmFreqHz, byte pwmPin1, byte pwmPin2, byte pwmPin3, byte pwmPin4, byte flags)
	: pwmFreqHz(pwmFreqHz), pwmPin1(pwmPin1), pwmPin2(pwmPin2), pwmPin3(pwmPin3), pwmPin4(pwmPin4), flags(flags) {
    calculateTop(); // Call the method to calculate the TOP value for Timer1 based on the frequency
}

// Calculate the top values for timers based on frequency
void UnoPWM::calculateTop() {
    // Timer1 is a 16-bit timer running at 16MHz, phase-correct mode (divided by 2), no prescaling
    tcnt1Top = 16000000 / (2 * pwmFreqHz);

    // Timer2 is an 8-bit timer, runs at 16MHz but uses a prescaler of 8 for PWM mode 7
    tcnt2Top = (byte)((16000000UL / (8UL * pwmFreqHz)) - 1);  //should be 79 for 25 khz
}


// Set up Timer1 for the specified PWM frequency
void UnoPWM::setupTimer() {
    // Set PWM pins as output so they can generate PWM signals
	if (flags & FLAG_ENABLE_PIN1) {
		pinMode(pwmPin1, OUTPUT);
	}
	if (flags & FLAG_ENABLE_PIN2) {
		pinMode(pwmPin2, OUTPUT);
	}
	if (flags & FLAG_ENABLE_PIN3) {
		pinMode(pwmPin3, OUTPUT);
	}
	if (flags & FLAG_ENABLE_PIN4) {
		pinMode(pwmPin4, OUTPUT);
	}
    
	//DEBUG_PRINTLN_F(F("Flags: %d"), (int)flags);
    // Setup the timers based on flags provided in the constructor
	if (flags & FLAG_CONFIG_TIMER1) {
		setupTimer1(); // Configure Timer1 for PWM
	}
	if (flags & FLAG_CONFIG_TIMER2) {
		setupTimer2(); // Configure Timer2 for PWM
	}
}


/*void UnoPWM::setupTimer2() {
	//Set PWM frequency to about 25khz on pin 3 (timer 2 mode 7, prescale 8, count to 79)
	TIMSK2 = 0;
	TIFR2 = 0;
	TCCR2A = (1 << COM2B1) | (1 << WGM21) | (1 << WGM20);
	TCCR2B = (1 << WGM22) | (1 << CS21);
	OCR2A = 79;
	OCR2B = 0;

	// Set Timer2 for fast PWM at ~25 kHz on Pin 11 (OC2A)
	TIMSK2 = 0;  // Disable Timer2 interrupts
	TIFR2 = 0;

	TCCR2A = (1 << COM2A1) | (1 << WGM21) | (1 << WGM20); // Fast PWM, non-inverting mode
	TCCR2B = (1 << WGM22) | (1 << CS21); // Prescaler 8, Fast PWM
	OCR2A = 79;  // Set frequency to ~25 kHz (16MHz / (8 * (1+79)))

  DEBUG_PRINTLN_F(F("Setup Timer 2"));
}*/

// Configure Timer1 for PWM output on pins 9 and 10
void UnoPWM::setupTimer1() {
    // Reset Timer1 registers to ensure a clean setup
    TCCR1A = 0;
    TCCR1B = 0;
    TCNT1  = 0;

    // Set Timer1 to phase-correct PWM mode with ICR1 as TOP value
    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << CS10); // No prescaler
    ICR1 = tcnt1Top; // Set the TOP value

    // Enable output on the required pins based on the provided flags
    if (flags & FLAG_ENABLE_PIN1) {
        //pinMode(pwmPin1, OUTPUT);
        TCCR1A |= (1 << COM1A1); // Enable PWM on pin 9 (OC1A)
    }
    if (flags & FLAG_ENABLE_PIN2) {
        //pinMode(pwmPin2, OUTPUT);
        TCCR1A |= (1 << COM1B1); // Enable PWM on pin 10 (OC1B)
    }
}

// Configure Timer2 for PWM output on pins 3 and 11
void UnoPWM::setupTimer2() {
    // Reset Timer2 registers before configuration
    TCCR2A = 0;
    TCCR2B = 0;
    TIMSK2 = 0;
    TIFR2 = 0;

    // Set Timer2 to fast PWM mode (Mode 7), OCR2A as TOP value
    TCCR2A = (1 << WGM21) | (1 << WGM20);
    TCCR2B = (1 << WGM22) | (1 << CS21); // Prescaler of 8
    OCR2A = tcnt2Top; // Set TOP value for Timer2

    // Enable output on the required pins based on the provided flags
    if (flags & FLAG_ENABLE_PIN3) {
        //pinMode(pwmPin3, OUTPUT);
        TCCR2A |= (1 << COM2B1); // Enable PWM on pin 3 (OC2B)
        //DEBUG_PRINTLN_F(F("Setup Timer 2A"));
    }
    if (flags & FLAG_ENABLE_PIN4) {
        //pinMode(pwmPin4, OUTPUT);
        TCCR2A |= (1 << COM2A1); // Enable PWM on pin 11 (OC2A)
        //DEBUG_PRINTLN_F(F("Setup Timer 2B"));
    }
}

// Set the duty cycle for a given pin (either pwmPin1 or pwmPin2)
void UnoPWM::setPwmDuty(byte duty, byte pwmPin) {
    // Calculate the duty cycle as a value proportional to the TOP value (ICR1)
    // This gives us the correct value to adjust the signal on the pins
    //word dutyCycle = (((word)duty / 100.0) * tcnt1Top); // Convert duty percentage to an actual count value

    word dutyCycle = (((word)duty / 100.0) * tcnt1Top); // For Timer1 (16-bit)
    byte dutyCycle2 = ((duty / 100.0) * tcnt2Top);  // For Timer2 (8-bit)
    
	#ifdef MK4SC_DEBUG
		// Debug output
		static unsigned long lastDebugTime = 0;

		if (DEBUG_TIME_ELAPSED(2000, lastDebugTime)) {
			DEBUG_PRINTLN_F(F("duty cycle: %d, on PIN: %d"), (pwmPin == pwmPin1 || pwmPin == pwmPin2) ? dutyCycle : dutyCycle2, pwmPin);

			DEBUG_RESET_TIME(lastDebugTime);
		}

	#endif

    // Check which pin is selected and set the corresponding OCR1 register (OCR1A for pin 1, OCR1B for pin 2)
	// Assign the calculated duty cycle to the correct register based on the pin
	if (pwmPin == pwmPin1) {
		OCR1A = (uint16_t)dutyCycle; // Set duty cycle for pin 9 (OC1A)
	} else if (pwmPin == pwmPin2) {
		OCR1B = (uint16_t)dutyCycle; // Set duty cycle for pin 10 (OC1B)
	} else if (pwmPin == pwmPin3) {
		OCR2B = dutyCycle2; // Set duty cycle for pin 3 (OC2B)
	} else if (pwmPin == pwmPin4) {
		OCR2A = dutyCycle2; // Set duty cycle for pin 11 (OC2A)
	}
}
