/***************************************************************************
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; version 2 of the License                *
 *                                                                         *
 *   This program 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 this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>		/* abs() */
#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_tc1.h"
#include "at91_twi.h"

/* Hard limits for ISR */
#define PULSE_MIN	0x0500
#define PULSE_MAX	0x0D00
#define PULSE_TIMEOUT	0x0F00

#define PULSE_CENTER	0x08C0
#define PULSE_SWITCH	0x01F0

/* moving average filters */
#define PULSE_FILTER_FAST	(1<<2)
#define PULSE_FILTER_SLOW	(1<<4)
#define PULSE_FILTER_DIFF	16		/* point to switch filters */
#define PULSE_MID_DIFF		50		/* minimum diff to center */

#define VALUE_RANGE		256

#define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))

struct channel_data {
	uint16_t width;
	uint16_t width_slow;
	uint16_t filter;	/* 0 - fast filter, 1 - slow filter */
	uint16_t min;		/* minimum value during calibration */
	uint16_t mid;		/* center value */
	uint16_t max;		/* maximum value */
};

/* check eeprom parameter size (uint16_t min/mid/max) */
#if ((MAX_CHANNELS * 3 * 2) != EE_RC_CAL_DATA_SIZE)
#error "invalid EE_RC_CAL_DATA_SIZE"
#endif

static struct channel_data ch_data[MAX_CHANNELS];
static uint32_t count, valid, cal_in_progress;

static void ppm_isr(void)
{
	static uint32_t i;

	/* RC Compare -> no TIOA1 edge for 2.5ms */
	uint32_t status = *AT91C_TC1_SR;

	if (status & AT91C_TC_CPCS) {
		/* average channel count */
		count = ((count * 7) + (i << 8)) / 8;

		/* at least 4 channels and a stable channel count */
		if ((ROUND_DIV256(count) == i) && (i >= 4)) {
			if (valid < 10)
				valid++;

		} else if (valid > 0) {
			valid--;
		}

		/* reset index */
		i = 0;
	}

	/* edge on TIOA1 */
	if (status & AT91C_TC_LDRAS) {
		/* get impulse width */
		uint16_t width = *AT91C_TC1_RA;

		/* valid range: 1 - 2ms */
		if (width > PULSE_MIN && width < PULSE_MAX) {
			if (i < ARRAY_SIZE(ch_data)) {
				/* calc both filters */
				ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
				ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;

				if (cal_in_progress) {
					/* use slow filter values, calc center */
					ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
					ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
					ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
				}
			}
			i++;
		}
	}
}

uint32_t rcontrol_getvalues(struct rc_values *rc)
{
	if (valid < 5)
		return 0;

	uint32_t i;
	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
	for (i = 0; i < cnt; i++) {
		/* switch between fast and slow filter */
		uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);

		/*
		 * transition fast -> slow filter
		 * slow filter is lagging behind, so give it a boost
		 */
		if (filter && !ch_data[i].filter && !cal_in_progress)
			ch_data[i].width_slow = ch_data[i].width;

		ch_data[i].filter = filter;

		uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;

		/* expand the value to +/- VALUE_RANGE */
		int32_t tmp = (uint32_t)(width - ch_data[i].mid) * VALUE_RANGE;
		tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));

		/* keep result in range */
		if (tmp > VALUE_RANGE)
			tmp = VALUE_RANGE;

		if (tmp < -VALUE_RANGE)
			tmp = -VALUE_RANGE;

		// TODO: stick mapping
		rc->chan[i] = tmp;
	}
	return cnt;
}

uint32_t rcontrol_getswitches(struct rc_values *rc)
{
	if (valid < 5)
		return 0;

	uint32_t i;
	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
	for (i = 0; i < cnt; i++) {
		if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
			rc->chan[i] = VALUE_RANGE;

		else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
			rc->chan[i] = -VALUE_RANGE;

		else
			rc->chan[i] = 0;
	}
	return cnt;
}

void rcontrol_calibrate(uint32_t mode)
{
	uint32_t i;
	uint8_t buf[EE_RC_CAL_DATA_SIZE];
	uint16_t *ptr = (uint16_t *)buf;

	switch (mode) {
	case RC_CAL_START:
		cal_in_progress = 1;
		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
			/* use hard limits as hint */
			ch_data[i].max = PULSE_MIN;
			ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
			ch_data[i].min = PULSE_MAX;
		}
		break;

	case RC_CAL_END:
		cal_in_progress = 0;
		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
			/* treat current position as center */
			ch_data[i].mid = ch_data[i].width_slow;

			/* if center is near minimum, clamp output to 0..+1024 */
			if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
				ch_data[i].mid = ch_data[i].min;

			/* if center is near maximum, clamp output to -1024..0 */
			if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
				ch_data[i].mid = ch_data[i].max;
		}
		break;

	case RC_CAL_LOAD:
		twi_read_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
			ch_data[i].min = *ptr++;
			ch_data[i].mid = *ptr++;
			ch_data[i].min = *ptr++;
		}
		break;

	case RC_CAL_SAVE:
		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
			*ptr++ = ch_data[i].min;
			*ptr++ = ch_data[i].mid;
			*ptr++ = ch_data[i].min;
		}
		twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
		break;
	}
}

void rcontrol_print_cal(void)
{
	uint32_t i;
	for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
		printf("%ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
			ch_data[i].min, ch_data[i].min - ch_data[i].mid,
			ch_data[i].mid,
			ch_data[i].max, ch_data[i].max - ch_data[i].mid
			);
	}
}

void at91_tc1_init(void)
{
	/* enable TC1 clock */
	*AT91C_PMC_PCER = (1 << AT91C_ID_TC1);

	/* MCK /32, trigger & capture on falling TIOA1 edge */
	*AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
			AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;

	/* enable RA load and RC compare interrupt */
	*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;

	/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
	*AT91C_TC1_RC = PULSE_TIMEOUT;

	/* enable & trigger the clock */
	*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;

	/* level triggered, own vector */
	AT91S_AIC *aic = AT91C_BASE_AIC;
	aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
	aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
	aic->AIC_IECR = (1 << AT91C_ID_TC1);

	rcontrol_calibrate(RC_CAL_LOAD);
	rcontrol_print_cal();
}