#include <avr/interrupt.h>
#include <avr/io.h>
#include <inttypes.h>
#include <string.h>

#include "atomic.h"
#include "timer0.h"
#include "midi.h"
#include "../../vst/vinylcontroller/src/midi_protocol.h"


#ifndef FALSE
# define FALSE  0
#endif
#ifndef TRUE
# define TRUE   (!FALSE)
#endif 

#ifndef MIN
# define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#endif
#ifndef MAX
# define MAX(a, b)  (((a) > (b)) ? (a) : (b))
#endif

#ifndef hi8
# define hi8(x)  ((uint8_t)((x) >> 8))
#endif
#ifndef lo8
# define lo8(x)  ((uint8_t)(x))
#endif


#ifndef F_CPU
# error F_CPU must be defined to the clock frequency in Hz
#endif


#define MIDI_DEFAULT_CHANNEL  15
#define MIDI_BAUDRATE         31250ul
#define MIDI_UBRR             ((F_CPU / (16ul * MIDI_BAUDRATE)) - 1ul)

#if (F_CPU / (16ul * (MIDI_UBRR + 1ul))) != MIDI_BAUDRATE
# error MIDI UBRR error!
#endif


#define TT_33RPM_INTERVAL  (F_CPU * 60 * 3 / 100 / TIMER0_PRESCALE)
#define TT_45RPM_INTERVAL  (F_CPU * 60 / 45 / TIMER0_PRESCALE)

#define TT_33RPM_TICK_INTERVAL  (TT_33RPM_INTERVAL / 100) // 2250 @ 8MHz
#define TT_45RPM_TICK_INTERVAL  (TT_45RPM_INTERVAL / 100)


#define HEAD_SERVO_PORT        PORTD
#define HEAD_SERVO_PIN         7
#define HEAD_SERVO_DDR         _SFR_IO8(_SFR_IO_ADDR(HEAD_SERVO_PORT) - 1)

#define HEAD_POSITION_RAISED   0
#define HEAD_POSITION_LOWERED  1


// FIXME FIXME FIXME check if stackpointer overruns


/******************** TYPES ********************/


typedef struct _MidiBuffer
{
  uint8_t  size;
  uint8_t  pos;
  uint8_t  data[4];
} MidiBuffer;


/******************** VARS ********************/


static struct
{
	struct
	{
		uint8_t  rpm;
	} turntable;
	struct
	{
		uint16_t  speed;  /* in µM/rev */
		uint16_t  leadInSpeed;  /* in µM/rev */
		uint16_t  leadInDuration;  /* 100 for one revolution */
		uint16_t  markSpeed;  /* in µM/rev */
		uint16_t  markDuration;  /* 100 for one revolution */
		uint16_t  leadOutSpeed;  /* in µM/rev */
		uint16_t  leadOutDuration;  /* 100 for one revolution */
	} pitch;
	struct
	{
		uint8_t  position;  /* HEAD_POSITION_XXX */
		uint8_t  raiseCurve[16];
		uint8_t  lowerCurve[16];
	} head;
} m_params =
{
	{ .rpm = 33 },
	{ .speed = 0,
	  .leadInSpeed = 1000, .leadInDuration = 500,
	  .markSpeed = 500, .markDuration = 100,
	  .leadOutSpeed = 5000, .leadOutDuration = 100 },
	{ .position = HEAD_POSITION_RAISED,
	  .raiseCurve = { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 },
	  .lowerCurve = { 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff } }
};


static struct
{
	uint8_t     channel;
	MidiBuffer  out;
	MidiBuffer  in;
	uint8_t     lastController;
	uint16_t    nrpn;
	uint8_t     dataMSB;
} m_midi =
{
	MIDI_DEFAULT_CHANNEL,
	{0, 0},
	{0, 0},
	0x00,
	MIDI_NRPN_RESET,
	0x00
};


static struct
{
	uint32_t  leadInEndTicks;
	uint32_t  markEndTicks;
	uint32_t  leadOutEndTicks;
} m_pitch =
{
	0, 0
};


static struct
{
	uint8_t  servoPulseLength;
	uint8_t  servoCurve;
	uint8_t  servoCurvePos;
} m_head =
{
	128,
	0,
	0
};

/******************** PROTOTYPES ********************/


void PITCH_LeadIn(void);
void PITCH_Mark(void);
void PITCH_LeadOut(void);
void HEAD_ChangePos(uint8_t newPos);


/******************** FUNCTIONS ********************/


/*** PARAMETERS ***/


void
PARAM_Init(void)
{
}


void
PARAM_Set(uint16_t nrpn, uint16_t value)
{
	switch (nrpn)
	{
	case MIDI_NRPN_TT_RPM:
		switch (value)
		{
		case 33:
			TIMER0_Start(TT_33RPM_TICK_INTERVAL);
			break;
		case 45:
			TIMER0_Start(TT_45RPM_TICK_INTERVAL);
			break;
		default:
			value = 0;
			TIMER0_Stop();
			break;
		}
		m_params.turntable.rpm = value;
		break;
	
	case MIDI_NRPN_PITCH_SPEED:
		m_params.pitch.speed = value;
		break;
	
	case MIDI_NRPN_PITCH_LEADIN_SPEED:
		m_params.pitch.leadInSpeed = value;
		break;
	
	case MIDI_NRPN_PITCH_LEADIN_DURATION:
		m_params.pitch.leadInDuration = value;
		break;
	
	case MIDI_NRPN_PITCH_MARK_SPEED:
		m_params.pitch.markSpeed = value;
		break;
	
	case MIDI_NRPN_PITCH_MARK_DURATION:
		m_params.pitch.markDuration = value;
		break;
	
	case MIDI_NRPN_PITCH_LEADOUT_SPEED:
		m_params.pitch.leadOutSpeed = value;
		break;
	
	case MIDI_NRPN_PITCH_LEADOUT_DURATION:
		m_params.pitch.leadOutDuration = value;
		break;
	
	case MIDI_NRPN_HEAD_POSITION:
//		m_params.head.position = (value < 0x2000) ? HEAD_POSITION_RAISED : HEAD_POSITION_LOWERED;
		HEAD_ChangePos((value < 0x2000) ? HEAD_POSITION_RAISED : HEAD_POSITION_LOWERED);
		break;
	
	case MIDI_NRPN_HEAD_SERVO_RAISE_CURVE:
		m_params.head.raiseCurve[value >> 8] = lo8(value);
		break;
	
	case MIDI_NRPN_HEAD_SERVO_LOWER_CURVE:
		m_params.head.lowerCurve[value >> 8] = lo8(value);
		break;
	}
}

#if 0
uint16_t
PARAM_Get(uint16_t nrpn)
{
	switch (nrpn)
	{
	case MIDI_NRPN_TT_RPM:
		return m_params.turntable.rpm;
		break;
	case MIDI_NRPN_PITCH_SPEED:
		return m_params.pitch.speed;
		break;
	case MIDI_NRPN_PITCH_MARK_SPEED:
		return m_params.pitch.markSpeed;
		break;
	case MIDI_NRPN_PITCH_MARK_DURATION:
		return m_params.pitch.markDuration;
		break;
	case MIDI_NRPN_PITCH_LEADIN_SPEED:
		return m_params.pitch.leadInSpeed;
		break;
	case MIDI_NRPN_PITCH_LEADIN_DURATION:
		return m_params.pitch.leadInDuration;
		break;
	case MIDI_NRPN_HEAD_POSITION:
		switch (m_params.head.position)
		{
		case HEAD_POSITION_RAISED:
			return 0x0000;
			break;
		case HEAD_POSITION_LOWERED:
			return 0x3fff;
			break;
		}
		break;
	}
	return 0;
}
#endif


/*** MIDI ***/


void
MIDI_Init(void)
{
	/* initialize USART */
	UBRRH = hi8(MIDI_UBRR);
	UBRRL = lo8(MIDI_UBRR);
	UCSRA = 0x00;
	UCSRB = _BV(RXCIE) | _BV(UDRIE) | _BV(RXEN) | _BV(TXEN);
	UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); /* asynchronous, no parity, 1 stop bit, 8 data bits */
}


void
MIDI_Send(const uint8_t *data, uint8_t size)
{
	register uint8_t s;
	while (size > 0)
	{
		while (m_midi.out.pos < m_midi.out.size);
		
		s = MIN(size, sizeof (m_midi.out.data) + 1);
		if (s > 1)
		{
			memcpy(m_midi.out.data, data + 1, s - 1);
			m_midi.out.size = s - 1;
			m_midi.out.pos = 0;
		}
		
		/* send first byte */
		loop_until_bit_is_set(UCSRA, UDRE);
		UDR = data[0];
		
		if (s > 1)
			UCSRB |= _BV(UDRIE); /* enable UDRE interrupt */
		
		data += s;
		size -= s;
	}
}


ISR(USART_UDRE_vect)
{
	PORTB ^= _BV(0);
	if (m_midi.out.pos < m_midi.out.size)
		UDR = m_midi.out.data[m_midi.out.pos++];
	else /* disable UDRE interrupt */
		UCSRB &= ~_BV(UDRIE);
}


void
MIDI_HandleData2(void)
{
	register uint8_t tmp = SREG;
	cli();
	#if 0 /* This is done in MIDI_HandleData */
	if (m_midi.in.pos <= 0) /* no data received so far */
	{
		SREG = tmp;
		return;
	}
	#endif
	
	if (m_midi.in.size <= 0) /* new message received, check status byte and set size of message */
	{
		register uint8_t data = m_midi.in.data[0];
		switch ((data & MIDI_MESSAGE_MASK) >> MIDI_MESSAGE_SHIFT)
		{
		case MIDI_MESSAGE_NOTEON:
		case MIDI_MESSAGE_CONTROLCHANGE:
			if (((data & MIDI_CHANNEL_MASK) >> MIDI_CHANNEL_SHIFT) != m_midi.channel)
				break;
			m_midi.in.size = 3;
			break;
		}
		if (m_midi.in.size <= 0) /* unknown message */
		{
			m_midi.in.pos = 0; /* ignore message */
			SREG = tmp;
			return;
		}
	}
	
	if (m_midi.in.pos < m_midi.in.size) /* message not completely received */
	{
		SREG = tmp;
	}
	else
	{
		static uint8_t message[4];
		
		/* copy message */
		memcpy(message, m_midi.in.data, m_midi.in.size);
		m_midi.in.pos = 0;
		
		/* reenable interrupts */
		SREG = tmp;
		
		//MIDI_Send(message, m_midi.in.size);
		
		/* handle message */
		switch ((message[0] & MIDI_MESSAGE_MASK) >> MIDI_MESSAGE_SHIFT)
		{
		case MIDI_MESSAGE_NOTEON:
			switch (message[1])
			{
			case MIDI_NOTE_PITCH_LEADIN:
				PITCH_LeadIn();
				break;
			case MIDI_NOTE_PITCH_MARK:
				PITCH_Mark();
				break;
			case MIDI_NOTE_PITCH_LEADOUT:
				PITCH_LeadOut();
				break;
			}
			break;
		
		case MIDI_MESSAGE_CONTROLCHANGE:
			switch (message[1])
			{
			case MIDI_CONTROLLER_NRPN_MSB:
				//PORTB ^= _BV(2);
				PORTB &= ~_BV(2);
				m_midi.nrpn &= ~(0x007f << 7);
				m_midi.nrpn |= message[2] << 7;
				break;
			case MIDI_CONTROLLER_NRPN_LSB:
				if (m_midi.lastController != MIDI_CONTROLLER_NRPN_MSB)
				{
					m_midi.nrpn = MIDI_NRPN_RESET;
					break;
				}
				//PORTB ^= _BV(2);
				PORTB |= _BV(2);
				m_midi.nrpn &= ~0x007f;
				m_midi.nrpn |= message[2];
				break;
			case MIDI_CONTROLLER_DATA_MSB:
				if (m_midi.lastController != MIDI_CONTROLLER_NRPN_LSB)
				{
					m_midi.nrpn = MIDI_NRPN_RESET;
					break;
				}
				//PORTB ^= _BV(2);
				PORTB &= ~_BV(2);
				m_midi.dataMSB = message[2];
				break;
			case MIDI_CONTROLLER_DATA_LSB:
				if (m_midi.lastController != MIDI_CONTROLLER_DATA_MSB)
				{
					m_midi.nrpn = MIDI_NRPN_RESET;
					break;
				}
				//PORTB ^= _BV(2);
				PORTB |= _BV(2);
				PARAM_Set(m_midi.nrpn, (m_midi.dataMSB << 7) | message[2]);
				break;
			break;
			}
			m_midi.lastController = message[1];
			break;
		}
	}
}


void __attribute__((naked))
MIDI_HandleData(void)
{
	__asm__(
	"	lds   __tmp_reg__, %0"				"\n\t"
	"	cpse  __tmp_reg__, __zero_reg__"	"\n\t"
	"	rjmp  MIDI_HandleData2"				"\n\t"
	"	ret"								"\n\t"
	: /* no output */
	: "m"(m_midi.in.pos)
	);
}


ISR(USART_RX_vect)
{
	register uint8_t data;
	
	PORTB ^= _BV(0);
	
	/* check for FE, DOR or PE and ignore the current message if any */
	if (UCSRA & (_BV(FE) | _BV(DOR) | _BV(PE)))
	{
		m_midi.in.pos = 0;
		data = UDR;
		return;
	}
	
	data = UDR;
	//UDR = data;
	if (data & MIDI_STATUS_FLAG)
	{
		m_midi.in.size = 0;
		m_midi.in.data[0] = data;
		m_midi.in.pos = 1;
	}
	else if (m_midi.in.pos > 0)
	{
		if (m_midi.in.pos < sizeof (m_midi.in.data))
			m_midi.in.data[m_midi.in.pos++] = data;
	}
}


/*** TURNTABLE ***/


void
TURNTABLE_Init(void)
{
	/* start timer */
	TIMER0_Start(TT_33RPM_TICK_INTERVAL);
}


/*** PITCH ***/


void
PITCH_Init(void)
{
	TCCR1A = _BV(COM1A1) | _BV(WGM11) | _BV(WGM10); /* Clear OC1A on compare, set at TOP, 10bit fast-pwm mode */
	TCCR1B = _BV(WGM12) | _BV(CS10); /* 10bit fast-pwm mode, no prescaling */
	OCR1A = 0;
	DDRD |= _BV(DDD5); /* set OC1A pin as output */
}


void
PITCH_UpdateSpeed(void)
{
	uint16_t speed, ticks;
	PORTB |= _BV(6) | _BV(5) | _BV(4);
	ticks = atomic_get_uint32(TIMER0_ticks);
	if (m_pitch.leadInEndTicks > ticks)
	{
		PORTB &= ~_BV(6);
		speed = m_params.pitch.leadInSpeed;
	}
	else if (m_pitch.markEndTicks > ticks)
	{
		PORTB &= ~_BV(6);
		speed = m_params.pitch.markSpeed;
	}
	else if (m_pitch.leadOutEndTicks > ticks)
	{
		PORTB &= ~(_BV(5) | _BV(6));
		speed = m_params.pitch.leadOutSpeed;
	}
	else
	{
		PORTB &= ~_BV(4);
		speed = m_params.pitch.speed;
	}
	OCR1A = MIN(0x03ff, speed);
}


void
PITCH_LeadIn()
{
	m_pitch.leadInEndTicks = atomic_get_uint32(TIMER0_ticks) +
	                         m_params.pitch.leadInDuration;
}


void
PITCH_Mark()
{
	m_pitch.markEndTicks = atomic_get_uint32(TIMER0_ticks) +
	                       m_params.pitch.markDuration;
}


void
PITCH_LeadOut()
{
	m_pitch.leadOutEndTicks = atomic_get_uint32(TIMER0_ticks) +
	                          m_params.pitch.leadOutDuration;
}


/*** HEAD ***/


void
HEAD_Init(void)
{
	HEAD_SERVO_PORT &= ~_BV(HEAD_SERVO_PIN);
	HEAD_SERVO_DDR |= _BV(HEAD_SERVO_PIN);
	
	/* start TIMER2 with prescale /32, overflow is every 1.024 ms @ 8MHz then */
	TCNT2 = 0;
	TIFR = _BV(TOV2);
	TIMSK |= _BV(TOIE2);
	TCCR2 = _BV(CS21) | _BV(CS20); /* prescale /32 */
}


void
HEAD_ServoPulse(uint8_t pos)
{
	atomic_set_uint8(m_head.servoPulseLength, pos);
}


ISR(TIMER2_OVF_vect)
{
	static uint8_t delay = 2 + 10; /* delay 10 * 1.024ms between pulses */
	if (--delay > 2)
		return;
	else if (delay == 2)
	{
		/* raise pin */
		HEAD_SERVO_PORT |= _BV(HEAD_SERVO_PIN);
	}
	else
	{
		if (delay == 1)
		{
			/* pin is raised for 1.024ms */
			if (m_head.servoPulseLength > 5)
			{
				TCNT2 -= m_head.servoPulseLength;
			}
			else
			{
				--delay;
			}
		}
		if (delay == 0)
		{
			/* pulse finished */
			HEAD_SERVO_PORT &= ~_BV(HEAD_SERVO_PIN);
			delay = 2 + 10;
			if (m_head.servoCurvePos < (sizeof (m_params.head.raiseCurve) / sizeof (m_params.head.raiseCurve[0])))
			{
				register uint8_t pos = m_head.servoCurvePos;
				if (m_head.servoCurve == 1)
					pos += sizeof (m_params.head.raiseCurve) / sizeof (m_params.head.raiseCurve[0]);
				m_head.servoPulseLength = m_params.head.raiseCurve[pos];
				m_head.servoCurvePos++;
			}
		}
	}
}


void
HEAD_ChangePos(uint8_t newPos)
{
	register uint8_t curve, tmp;
	
	if (m_params.head.position == newPos)
		return;
	
	m_params.head.position = newPos;
	if (newPos == HEAD_POSITION_LOWERED)
	{
		PORTB &= ~_BV(3);
		//HEAD_ServoPulse(0xff);
		curve = 1;
	}
	else
	{
		PORTB |= _BV(3);
		//HEAD_ServoPulse(0x00);
		curve = 0;
	}
	
	tmp = SREG;
	cli();
	m_head.servoCurve = curve;
	m_head.servoCurvePos = 0;
	SREG = tmp;
}


/*** MAIN ***/


void
UNUSED_Init(void)
{
	TIMSK = 0x00;
	/* TODO: Set all floating pins as output! */
}


void
main(void)
{
	uint32_t next;
	
	/* init */
	UNUSED_Init();
	PARAM_Init();
	MIDI_Init();
	TURNTABLE_Init();
	PITCH_Init();
	HEAD_Init();
	sei();
	
	DDRB = 0xff;
	PORTB = 0xff;
	
	next = atomic_get_uint32(TIMER0_ticks) + 100;
	for(;;)
	{
		MIDI_HandleData();
		PITCH_UpdateSpeed();
		
		if (atomic_get_uint32(TIMER0_ticks) >= next)
		{
			next += 100;
			PORTB ^= _BV(7);
		}
	}
}