/***************************************************************************
 *   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 "AT91SAM7S256.h"
#include "board.h"
#include "at91_adc.h"
#include "at91_pitc.h"

#include "fixed.h"

#define ADC_CAL_COUNT	1024

static uint16_t adc_tmp[7];
static int16_t adc_result[7];
static uint16_t adc_offset[6];

static uint32_t adc_cal_count;
volatile static uint32_t adc_cal_mode;
static uint32_t adc_cal_data[3];

static void at91_adc_isr(void)
{
	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
	pdc->PDC_RPR = (uint32_t) &adc_tmp;
	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
	pdc->PDC_PTCR = AT91C_PDC_RXTEN;

	/* clear interrupts */
	uint32_t status = *AT91C_ADC_SR;
	status = status;

	uint32_t i;
	for (i = 0; i < ARRAY_SIZE(adc_offset); i++)
		adc_result[i] = adc_tmp[i] - adc_offset[i];

	/* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
	adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
}

static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
{
	/* trigger next cycle */
	*AT91C_ADC_CR = AT91C_ADC_START;

	if (adc_cal_mode == ADC_CAL_GYRO) {
		adc_cal_data[0] += adc_tmp[ADC_GYRO_ROLL];
		adc_cal_data[1] += adc_tmp[ADC_GYRO_NICK];
		adc_cal_data[2] += adc_tmp[ADC_GYRO_YAW];

	} else {
		adc_cal_data[0] += adc_tmp[ADC_ACC_ROLL];
		adc_cal_data[1] += adc_tmp[ADC_ACC_NICK];
		adc_cal_data[2] += adc_tmp[ADC_ACC_YAW];
	}

	adc_cal_count--;
	if (adc_cal_count == 0) {
		adc_cal_data[0] /= ADC_CAL_COUNT;
		adc_cal_data[1] /= ADC_CAL_COUNT;
		adc_cal_data[2] /= ADC_CAL_COUNT;

		if (adc_cal_mode == ADC_CAL_GYRO) {
			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
			adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
			adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];

			// TODO: check for invalid values (not centered) -> return value?

			printf("GYRO offsets: %4d/%4d/%4d\n\r",
				adc_offset[ADC_GYRO_ROLL],
				adc_offset[ADC_GYRO_NICK],
				adc_offset[ADC_GYRO_YAW]);

		} else {
			adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
			adc_offset[ADC_ACC_NICK] = adc_cal_data[1];

			// TODO: only 1/2 offset?
			adc_offset[ADC_ACC_YAW] = adc_cal_data[2];

			// TODO: save to eeprom
			printf("ACC  offsets: %4d/%4d/%4d\n\r",
				adc_offset[ADC_ACC_ROLL],
				adc_offset[ADC_ACC_NICK],
				adc_offset[ADC_ACC_YAW]);
		}

		adc_cal_mode = ADC_CAL_NONE;
		return PITC_REMOVE_TIMER;
	}

	return PITC_RESTART_TIMER;
}

static struct pitc_timer adc_cal_timer = {
	.interval = 1,
	.func = &adc_calibrate_cb,
};

void adc_trigger(void)
{
	printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
		adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
		adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
		adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
		adc_result[ADC_VOLTAGE]);

	*AT91C_ADC_CR = AT91C_ADC_START;
}

void adc_calibrate(uint32_t mode)
{
	if (adc_cal_mode != ADC_CAL_NONE)
		return;

	if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
		adc_cal_count = ADC_CAL_COUNT;
		adc_cal_mode = mode;
		adc_cal_data[0] = 0;
		adc_cal_data[1] = 0;
		adc_cal_data[2] = 0;
		pitc_schedule_timer(&adc_cal_timer);
	}
}

void at91_adc_init(void)
{
	/* enable ADC clock */
	*AT91C_PMC_PCER = (1 << AT91C_ID_ADC);

	// TODO: load calibration data from eeprom / use default values

	/* ADC Software reset */
	AT91S_ADC *adc = AT91C_BASE_ADC;
	adc->ADC_CR = AT91C_ADC_SWRST;

	/*
	 * ADC config: 10bit, no sleep
	 * 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz)
	 * 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us)
	 * 3 cycles SH ((2 +1) / 4.8MHz = 625ns)
	 * Conversion time per channel @5MHz ~2us
	 */
	adc->ADC_MR = AT91C_ADC_TRGEN_DIS |
			AT91C_ADC_LOWRES_10_BIT |
			AT91C_ADC_SLEEP_NORMAL_MODE |
			(AT91C_ADC_PRESCAL & (4 << 8)) |
			(AT91C_ADC_STARTUP & (11 << 16)) |
			(AT91C_ADC_SHTIM & (2 << 24));

	/* setup PDC */
	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
	pdc->PDC_RPR = (uint32_t) &adc_tmp;
	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
	pdc->PDC_PTCR = AT91C_PDC_RXTEN;

	/* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
	adc->ADC_CHER = 0xF7;
	adc->ADC_IER = AT91C_ADC_ENDRX;

	/* low priority, level triggered, own vector */
	AT91S_AIC *aic = AT91C_BASE_AIC;
	aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
	aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
	aic->AIC_IECR = (1<<AT91C_ID_ADC);
}