/*UVOS*/

/* This file is part of UsbLedController.
 *
 * TelemetrySystem is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License (LGPL) version 3 as published by
 * the Free Software Foundation.
 *
 * TelemetrySystem is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with TelemetrySystem.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <stdbool.h>
#include "writepin.h"
#include "usbdrv.h"

#define SOFT_PWM_DIV 1
static volatile uint16_t counter;
static volatile uint16_t lastTime;
static volatile uint8_t ovfCount[4] = {0xFF,0xFF,0xFF,0xFF};

static int16_t cameraOffset;
static uint8_t mask;
static uint8_t current;
static uint16_t intvl;

static uint8_t runSeq;

enum {
	SEQ_NONE = 0,
	SEQ_TRIG,
	SEQ_PIC,
};


ISR(TIM1_COMPA_vect)
{
	PORTA = PORTA & (1 << PA4) ? PORTA & ~(1 << PA4) : PORTA | (1 << PA4);
	uint16_t intrTime = TCNT1;
	counter += lastTime <= intrTime ? (intrTime - lastTime)/SOFT_PWM_DIV : (intrTime + (UINT16_MAX-counter))/SOFT_PWM_DIV;
	lastTime = intrTime;
	
	uint8_t next = UINT8_MAX-counter+1; //155
	if(next == 0) next = UINT8_MAX;
	uint8_t update = PORTA & 0xF0;
	for (uint8_t i = 0; i < 4; ++i)
	{
		if(counter < ovfCount[i])
		{
			update |= 1 << i;
			if(next > ovfCount[i]-counter)
				next = ovfCount[i]-counter;
		}
	}
	PORTA = update;
	
	if(next < 32)
		next = 32;

	uint32_t next32 = ((uint32_t)next)*SOFT_PWM_DIV+intrTime;
	OCR1A = next32 < UINT16_MAX ? next32 : next32 - (UINT16_MAX-intrTime);
}


void poll()
{
	usbPoll();
	
	uint8_t update = PORTA & 0xF0;
	uint8_t time = TCNT0;
	for (uint8_t i = 0; i < 4; ++i)
	{
		if(time < ovfCount[i] || ovfCount[i] == UINT8_MAX)
		{
			update |= 1 << i;
		}
	}
	PORTA = update;
}

void pollWait(uint16_t ms)
{
	ms *= (F_CPU/256)/1000;
	TCNT1 = 0;
	while(TCNT1 < ms)
		poll();
}

static void triggerCamera(void)
{
	writePin(&PORTB, PB2, true);
	pollWait(50);
	writePin(&PORTB, PB2, false);
}

static void takePicture(const uint8_t mask, const uint8_t current, const uint16_t interval, const int16_t cameraOffset)
{
	
	if(cameraOffset > interval)
		return;
	
	if(cameraOffset < 0)
	{
		writePin(&PORTB, PB2, true);
		pollWait((uint16_t)(0 - cameraOffset));
	}

	for(uint8_t i = 0; i < 4; ++i)
	{
		if(mask & (1 << i))
			ovfCount[i] = UINT8_MAX - current;
	}
	
	if(cameraOffset > 0)
	{
		pollWait((uint16_t)cameraOffset);
		writePin(&PORTB, PB2, true);
		pollWait(interval - cameraOffset);
	}
	else
	{
		pollWait(interval);
	}
	
	for(uint8_t i = 0; i < 4; ++i)
	{
		if(mask & (1 << i))
			ovfCount[i] = 255;
	}
	
	writePin(&PORTB, PB2, false);
}


usbMsgLen_t usbFunctionSetup(uchar data[8])
{
	usbRequest_t    *rq = (usbRequest_t *)data;
	static uchar    dataBuffer[8]; 

	if(rq->bRequest == 0)
	{
		writePin(&PORTA, PA5, false);
		return 0;
	}
	
	if(rq->bRequest == 1)
	{
		writePin(&PORTA, PA5, true);
		return 0;
	}
	
	if(rq->bRequest == 2)
	{
		for (uint8_t i = 0; i < 4; ++i)
		{
			if(rq->wValue.bytes[0] & (1 << i))
				ovfCount[i] = UINT8_MAX - rq->wValue.bytes[1];
		}
		return 0;
	}
	
	if(rq->bRequest == 3)
	{
		triggerCamera();
		return 0;
	}
	
	if(rq->bRequest == 4)
	{
		mask = rq->wValue.bytes[0];
		current = rq->wValue.bytes[1];
		intvl = rq->wIndex.word;
		return 0;
	}

	if(rq->bRequest == 5)
	{
		cameraOffset = rq->wValue.word;
		return 0;
	}
	
	if(rq->bRequest == 128)
	{
		dataBuffer[0] = 55;
		dataBuffer[1] = 55;
		dataBuffer[2] = 55;
		dataBuffer[3] = 55;
		usbMsgPtr = (short unsigned int)dataBuffer;
		return 4;
	}
	return 0; 
}

static void timer1InterruptEnable(const bool enable)
{
	TIMSK1 = enable ? TIMSK1 | (1 << OCIE1A) : TIMSK1 & ~(1 << OCIE1A);
}

int main(void)
{
	PORTA = (1 << PA0) | (1 << PA1) | (1 << PA2) | (1 << PA3) | (1 << PA4) | (1 << PA5);
	DDRA  = (1 << PA0) | (1 << PA1) | (1 << PA2) | (1 << PA3) | (1 << PA4) | (1 << PA5);
	DDRB  = (1 << PB2);
	PORTB = (1 << PB2);
	TCCR1B = (1<<CS12);
	TCCR0B = (1<<CS11);
	usbInit();
	usbDeviceDisconnect();  // enforce re-enumeration.
	_delay_ms(250);
	
	usbDeviceConnect();
	
	sei();
	TCNT1 = 0;
	while(true)
	{
		switch(runSeq)
		{
			case SEQ_TRIG:
				triggerCamera();
				break;
			case SEQ_PIC:
				takePicture(mask, current, intvl, cameraOffset);
				break;
			default:
				break;
		}
		runSeq = SEQ_NONE;
		poll();
	}
}