/*
 *
 * Created: 26.08.2021 12:01:13
 * Author : Joachim J.
 * MC: ATmega 328p mit 16MHz / Arduino Nano
 */ 

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <util/atomic.h>

/*-------- Define --------*/
#define FOSC 16000000 //Clock Speed
#define BAUD 19200 //Debug
#define MYUBRR FOSC/16/BAUD-1

#define LED_PIN PC0
#define LED_ON PORTC |= (1 << LED_PIN)
#define LED_OFF PORTC &= ~(1 << LED_PIN)

#define set_cap_low_high TCCR1B |= (1 << ICES1); //Bit setzen: Capture von low zu high / rising
#define set_cap_high_low TCCR1B &= ~(1 << ICES1); //Bit löschen: Capture von high zu low / falling

#define RC_RECEIVER_PORT PD2
#define RISING_EDGE PIND & (1 << RC_RECEIVER_PORT)

#define PPM_min 1145
#define PPM_max 1932

/*Variablen*/
volatile uint16_t Throttle;
volatile uint8_t ppm_ready=0;
volatile uint8_t cap_low_high;


void USART_Init( unsigned int ubrr)
{
	/*Set baud rate */
	UBRR0H = (unsigned char)(ubrr>>8);
	UBRR0L = (unsigned char)ubrr;
	/*Enable receiver and transmitter */
	UCSR0B = (1<<RXEN0)|(1<<TXEN0);
	/* Set frame format: 8data, 1stop bit */
	UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
}

void debug_transmit(unsigned char data)
//Nur für Debug
{
	// Wait for empty transmit buffer
	while ( !( UCSR0A & (1<<UDRE0)) );
	// Put data into buffer, sends the data
	UDR0 = data;
	//return 0;
}

void debug_puts (char *s)
{
	while (*s)
	{   // so lange *s != '\0' also ungleich dem "String-Endezeichen(Terminator)"
		debug_transmit(*s);
		s++;
	}
}

void pin_change_init(void)
{
	EICRA = (1<<ISC00);
	EIMSK = (1<<INT0);
}

void input_capture_init(void)
//ICP läuft mit 2 MHz: Servo Signal wird zu hoch aufgelöst. Umrechnung in Main. 
{
		TCNT1 = 0;             	//Rücksetzen des Timers
		TCCR1B = (1<<CS11);		//Timer an / Prescaler 8
		TCCR1B|=(1<<ICNC1)|(1<<ICES1); //Input Capture Noise Canceler enable, Input Capture Edge Select: low-->high
		TIMSK1 = (1<<ICIE1); 	//Timer/Counter1, Output Compare A Match Interrupt Enable
		cap_low_high=1;
		ppm_ready=0;
}

void PWM_Timer0_init (void)
{
	TCCR0A = (1<<COM0A1)|(1<<COM0A0)|(1<<COM0B1)|(1<<COM0B0)|(1<<WGM01)|(1<<WGM00); //Clear OC0A/OC0B on Compare Match, set OC0A/OC0B at BOTTOM (inverting mode) Fast PWM, Top=0xFF
	TCCR0B = (1<<CS01); //Prescaler = 8 --> 10 kHz für Motoren
	OCR0A=0; //Motor1 aus
	OCR0B=0; //Motor2 aus
}

void PWM_Timer2_init (void)
{
	TCCR2A = (1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)|(1<<WGM21)|(1<<WGM20); //Clear OC0A/OC0B on Compare Match, set OC0A/OC0B at BOTTOM (inverting mode) Fast PWM, Top=0xFF
	TCCR2A = (1<<CS21); //Prescaler = 8 --> ca. 10 kHz für Motoren
	OCR2A=0; //Motor1 aus
	OCR2B=0; //Motor2 aus
}

void PWM_init (void)
{
	TCCR0A = TCCR2A = 0xF3;
	TCCR0B = TCCR2B = 0x02;
	OCR0A = OCR0B = OCR2A = OCR2B = 0;
}

ISR (TIMER1_CAPT_vect)
{
	if (cap_low_high==1) //steigende Flanke
	{
		TCNT1=0;
		set_cap_high_low;
		LED_ON;
		cap_low_high=0;
	}
	
	else //fallende Flanke
	{
		Throttle=ICR1;
		set_cap_low_high;
		LED_OFF;
		cap_low_high=1;
		ppm_ready=1;
	}
}

int main()

{
	char Buffer[12];
	uint16_t Throttle_l;
	uint8_t M1=0;
	
	DDRD = (1<<DDD6)|(1<<DDD5)|(1<<DDD3); //Ausgänge
	DDRC = (1<<DDC1)|(1<<DDC0); //Ausgänge
	
	USART_Init(MYUBRR);
	_delay_ms(10);
	
	debug_puts("Start\r\n"); //Debug
	debug_puts("Init Timers\r\n"); //Debug
	
	
	input_capture_init();
	PWM_init();
	
	//debug
	OCR0A= 64;
	_delay_ms(2000);
	OCR0A= 128;
	_delay_ms(2000);
	OCR0A= 255;
	_delay_ms(2000);
	OCR0A= 0;

	sei();
	
	//Main
	while(1)
	{
		if (ppm_ready)
		{
			ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
			{
				Throttle_l = Throttle;
			}
			//Auf PPM umrechnen. Über ICP kommen für das Servo Signal Werte zwischen 2000 und 4000, da der ICP mit 2 MHZ läuft
			Throttle_l=Throttle_l/2;

			if (Throttle_l>PPM_max)
			{
				Throttle_l=PPM_max;
			}
			
			if (Throttle_l<PPM_min)
			{
				Throttle_l=PPM_min;
			}

			Throttle_l = Throttle_l-PPM_min;
	
			//Auf 8 bit skalieren 
			M1 = Throttle_l/3.1;
			
			//Debug: Wert für Motor seriell ausgeben
			utoa(M1,Buffer,10);
			debug_puts(Buffer); 
			debug_puts("\r\n");
			
			//Ausgabe über Hardware PWM
			OCR0A= M1;
						
			ppm_ready=0;
		}
		
	}
}