#include "libarmep.h"

#include <linux/serial.h>

int somebody_triggered = -1;
struct aep_channel *who_triggered = NULL;

static void track_limits_convert_to_current(struct aep_channel *ch,
							double *v1, double *v2)
{
	double pwr;

	if ((*v1 - ch->voffset[0]) < ch->min[3])
		ch->min[3] = *v1 - ch->voffset[0];

	if ((*v1 - ch->voffset[0]) > ch->max[3])
		ch->max[3] = *v1 - ch->voffset[0];

	if ((*v2 - ch->voffset[1]) < ch->min[4])
		ch->min[4] = *v2 - ch->voffset[1];

	if ((*v2 - ch->voffset[1]) > ch->max[4])
		ch->max[4] = *v2 - ch->voffset[1];

	if (!ch->aep->aep_context->auto_zero) {
		if (*v1 < 0) {
			if (ch->samples_seen > IGNORE_NEG_PRIOR_TO_SAMPLES &&
						  -(*v1) > ch->vnoise[0] / 2) {
				if (ch->aep->aep_context->verbose)
					fprintf(stderr, "*** reducing voltage "
						"baseline by %fV to %fV\n",
						*v1, ch->voffset[0] - *v1);
				ch->voffset[0] -= *v1 *
					NEGATIVE_ADJUST_SCALING_FACTOR;
			}
			*v1 = 0;
		}
		if (*v2 < 0) {
			if (ch->samples_seen > IGNORE_NEG_PRIOR_TO_SAMPLES &&
						  -(*v2) > ch->vnoise[1] / 2) {
				if (ch->aep->aep_context->verbose)
					fprintf(stderr, "*** reducing shunt "
						"voltage baseline by "
						"%fV to %fV\n", *v2,
						ch->voffset[1] - *v2);
				ch->voffset[1] -= *v2 *
					NEGATIVE_ADJUST_SCALING_FACTOR;
			}
			*v2 = 0;
		}
	}

	*v2 = correct(ch->aep->aep_context->no_correction, *v1, *v2, ch->map_table->map, ch->map_table->len) / ch->rshunt;

	pwr = *v1 * *v2;
	if (*v1 > ch->max[0])
		ch->max[0] = *v1;
	if (*v1 < ch->min[0])
		ch->min[0] = *v1;
	if (*v2 > ch->max[1])
		ch->max[1] = *v2;
	if (*v2 < ch->min[1])
		ch->min[1] = *v2;
	if (pwr > ch->max[2])
		ch->max[2] = pwr;
	if (pwr < ch->min[2])
		ch->min[2] = pwr;
}

int process_sample(struct aep *aep, int ch_index)
{
	struct aep_channel *ch = &aep->ch[ch_index], *ich;
	double v1, v2, v1a, v2a, vo0 = 0, vo1 = 0;
	int m, i, from_pretrig = 0;

	if (!aep->aep_context->no_correction) {
		vo0 = ch->voffset[0];
		vo1 = ch->voffset[1];
	}

	if (ch->pretrig_ring) {
		ch->pretrig_ring[ch->head].v = aep->voltage;
		ch->pretrig_ring[ch->head].i = aep->current;
		ch->head++;
		if (ch->head == ch->ring_samples)
			ch->head = 0;
		if (ch->head == ch->tail)
			ch->tail++;
		if (ch->tail == ch->ring_samples)
			ch->tail = 0;

		if (ch->pretrigger_samples_taken < ch->ring_samples)
			ch->pretrigger_samples_taken++;
	}

	if ((ch->triggered && aep->voltage < aep->aep_context->mv_min && ch->samples > 1000 && !ch->trigger_slave) ||
		(ch->trigger_slave && !ch->trigger_slave->triggered)) {

		ch->trigger_filter++;
		if (ch->trigger_filter < aep->aep_context->end_trigger_filter_us / 100)
			goto unripe;

		if (aep->aep_context->verbose)
			fprintf(stderr, "trigger session completed "
				"%dmV vs %dmV\n", aep->voltage, aep->aep_context->mv_min);
		avg_mean_us_flush(&ch->avg_mean_voltage);
		avg_mean_us_flush(&ch->avg_mean_current);
		// ch->ignore = 0;
		ch->triggered = 0;
		ch->trigger_filter = 0;
		ch->pretrigger_samples_taken = 0;
		ch->head = 0;
		ch->tail = 0;
		if (aep->aep_context->require_off) {
			if (aep->aep_context->verbose)
				fprintf(stderr, "marking capture for "
					"device %d done\n", aep->index);
			aep->aep_context->awaiting_capture &= ~(1 << (aep->index * 3 + ch_index));
		}
		return 0;
	} else
		if (ch->triggered)
			ch->trigger_filter = 0;
unripe:

	ch->samples_seen++;

	if (ch->ignore)
		return 0;

	if (!ch->triggered) {
		avg_mean_us_add(&ch->avg_mean_voltage, aep->voltage);
		avg_mean_us_add(&ch->avg_mean_current, aep->current);

		v1 = (avg_mean_us(&ch->avg_mean_voltage) /
						ADC_COUNTS_PER_VOLT_CH1) + vo0;
		v2 = (avg_mean_us(&ch->avg_mean_current) /
						ADC_COUNTS_PER_VOLT_CH2) + vo1;
		if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw)
			fprintf(stderr, "%fmV raw shunt;  corr=%fmV\n", v2,
				correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map,
							ch->map_table->len));


		track_limits_convert_to_current(ch, &v1, &v2);

		if ((((aep->voltage > aep->aep_context->mv_min + TRIG_HYSTERESIS_MV) || !aep->aep_context->mv_min) &&
		      (((v1 * v2) > (aep->aep_context->mw_min_plus_hyst) / 1000.0) || !aep->aep_context->mw_min)) ||
			(somebody_triggered != -1 && ch->samples_seen >= somebody_triggered)) {

			ch->trigger_filter++;
			if (ch->trigger_filter > aep->aep_context->trigger_filter_us / 100 || somebody_triggered != -1) {

				if (somebody_triggered != -1) {
					ch->trigger_slave = who_triggered;
				} else {
					who_triggered = ch;
					somebody_triggered = ch->samples_seen;							
				}
				ch->triggered = 1;
				ch->simple_avg[0] = 0;
				ch->simple_avg[1] = 0;
				ch->simple_avg[2] += 0;
				ch->avg_count = 0;
				ch->samples = 0;
				ch->trigger_filter = 0;
				if (aep->aep_context->verbose)
					fprintf(stderr, "%s: triggered "
						"%.2fV %.5fW pretrig %d "
						"(%dms)\n",
						ch->channel_name, v1,
						v1 * v2,
						ch->ring_samples,
						ch->ring_samples / 10);
			}
		} else
			ch->trigger_filter = 0;

		aep->rx_count++;
	}

	if (!ch->samples && aep->aep_context->ms_holdoff)
		fprintf(stderr, "%s: post-trigger holdoff for"
				     " %dms\n", ch->channel_name, aep->aep_context->ms_holdoff);

	if (ch->triggered) {
		ch->samples++;

		if (ch->samples / 10 < aep->aep_context->ms_holdoff)
			return 0;
		if (ch->samples - 1 == aep->aep_context->ms_holdoff * 10 && aep->aep_context->verbose) 
			fprintf(stderr, "%s: Starting capture\n", ch->channel_name);
	}

	if (ch->pretrig_ring) {
		from_pretrig = 1;
		avg_mean_us_add(&ch->avg_mean_voltage,
						ch->pretrig_ring[ch->tail].v);
		avg_mean_us_add(&ch->avg_mean_current,
						ch->pretrig_ring[ch->tail].i);
		ch->tail++;
		if (ch->tail == ch->ring_samples)
			ch->tail = 0;
	} else {
		avg_mean_us_add(&ch->avg_mean_voltage, aep->voltage);
		avg_mean_us_add(&ch->avg_mean_current, aep->current);
	}
	v1 = (avg_mean_us(&ch->avg_mean_voltage) /
						ADC_COUNTS_PER_VOLT_CH1) + vo0;
	v2 = (avg_mean_us(&ch->avg_mean_current) /
						ADC_COUNTS_PER_VOLT_CH2) + vo1;

	if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw)
		fprintf(stderr, "%fmV raw shunt;  corr=%fmV\n", v2,
		      correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map, ch->map_table->len));

	track_limits_convert_to_current(ch, &v1, &v2);

	if (aep->aep_context->auto_zero && v1 > REJECT_AUTOZERO_VOLTAGE) {
		fprintf(stderr, "**** Autozero seeing > %fV... "
			"probe should not be in powered target for "
			"autozero! ****\n", REJECT_AUTOZERO_VOLTAGE);
		return -2;
	}

	if (aep->aep_context->auto_zero) {
		ch->simple_avg[0] += v1;
		ch->simple_avg[1] += v2;
		ch->simple_avg[2] += v1 * v2;
		ch->avg_count++;
	}

	ch->decimation_counter++;
	if (ch->decimation_counter != aep->aep_context->decimate)
		goto done;
	ch->decimation_counter = 0;

	if (aep->aep_context->auto_zero) {
		fprintf(stderr, "Autozero for %s done         \n",
							     ch->channel_name);
		if (v1 > IGNORE_AUTOZERO_VOLTAGE || v1 < -IGNORE_AUTOZERO_VOLTAGE)
			fprintf(stderr, "   Voltage Autozero skipped as %fV "
				"unfeasibly high\n", v1);
		else {
			ch->voffset[0] = -v1;
			ch->vnoise[0] = ch->max[3] - ch->min[3];
		}
		ch->voffset[1] = -avg_mean_us(&ch->avg_mean_current) /
						ADC_COUNTS_PER_VOLT_CH2;
		ch->vnoise[1] = ch->max[4] - ch->min[4];
		if (configure(aep->aep_context, aep, aep->dev_filepath, aep->aep_context->config_filepath, ch) < 0)
			fprintf(stderr,
				"Failed to write autozero info to config\n");
		goto done;
	}
	
	/* not everyone else may be ready, save them up */
	if (v1 < 0)
		v1 = 0;
	if (v2 < 0)
		v2 = 0;
	ch->out_ring[0][ch->out_head] = v1;
	ch->out_ring[1][ch->out_head++] = v2;
	if (ch->out_head == MAX_RESULT_QUEUE_DEPTH)
		ch->out_head = 0;
	if (ch->out_head == ch->out_tail)
		fprintf(stderr, "\n***** %s output ring overflow...%d %d\n",
			ch->channel_name, ch->out_head, ch->out_tail);

	/* the other capture channels have data for this sample? */

	for (m = 0; m <= aep->aep_context->highest; m++) {
		if (!aep->aep_context->aeps[m].fd)
			continue;
		for (i = 0; i < CHANNELS_PER_PROBE; i++)
			if (aep->aep_context->aeps[m].ch[i].out_head == aep->aep_context->aeps[m].ch[i].out_tail) {
//				fprintf(stderr, "empy %s\n", aep->aep_context->aeps[m].ch[i].channel_name);
				goto done;
			}
	}

	/* if so, output it all together */

	aep->aep_context->samples(AEPSA_START_PRE + (ch->triggered * 3), ch, ((double)ch->samples -
					(double)ch->ring_samples) / 10000.0, 0);

	for (m = 0; m <= aep->aep_context->highest; m++) {
		if (!aep->aep_context->aeps[m].fd)
			continue;
		for (i = 0; i < CHANNELS_PER_PROBE; i++) {

			ich = &aep->aep_context->aeps[m].ch[i];

			v1a = ich->out_ring[0][ich->out_tail];
			v2a = ich->out_ring[1][ich->out_tail];
			ich->out_tail++;
			if (ich->out_tail == MAX_RESULT_QUEUE_DEPTH)
				ich->out_tail = 0; 
			if (ch->samples >= ch->ring_samples && !from_pretrig) {
				ich->simple_avg[0] += v1a;
				ich->simple_avg[1] += v2a;
				ich->simple_avg[2] += v1a * v2a;
				ich->avg_count++;
			}
			if (!ich->requested)
				continue;
	
			aep->aep_context->samples(AEPSA_SAMPLE_PRE + (ch->triggered * 3), ch, v1a, v2a);
		}
	}
		
	aep->aep_context->samples(AEPSA_END_PRE + (ch->triggered * 3), ch, 0, 0);

done:

	if (aep->aep_context->ms_capture && (ch->samples / 10 - aep->aep_context->ms_holdoff) > aep->aep_context->ms_capture) {
		fprintf(stderr, "%s: %dms capture complete  ",
				    ch->channel_name, aep->aep_context->ms_capture);
		ch->ignore = 1;
		if (!aep->aep_context->require_off) {
			aep->aep_context->awaiting_capture &= ~(1 << (aep->index * 3 + ch_index));
			fprintf(stderr, "\n");
		} else
			fprintf(stderr, "(waiting for OFF)\n");
	}

	return 0;
}

static int service_aep_buffers(struct aep *aep, int samples)
{
	unsigned char v;
	static unsigned char cmd[] = {
		AEPC_CONFIG, 0, 6,
	};
	int bad = 0;

	while (aep->head != aep->tail && samples--) {

		v = aep->ring[aep->head++];
		if (aep->head == sizeof(aep->ring))
			aep->head = 0;

		if (aep->aep_context->verbose)
			fprintf(stderr, "%02X ", v);

		switch (aep->state) {

		/*
		 * this filters the input to sync, it will keep returning to
		 * look for the first byte if any of AC FF FF FF FF FF FF FF FF
		 * is not correctly seen.
		 */

		case APP_INIT_MAGIC:
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					aep->counter = 0;
				break;
			case 1:
			case 2:
			case 3:
			case 4:
			case 5:
			case 6:
			case 7:
			case 8:
				if (v != 0xff)
					aep->counter = 0;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
					     "magic ok, asking for version\n");
				v = AEPC_VERSION;
				if (write(aep->fd, &v, 1) != 1)
					return -1;
				aep->state++;
				aep->counter = 0;
				break;
			default:
				break;
			}
			break;

		case APP_INIT_VERSION:
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 1:
				aep->version = v;
				break;
			case 2:
				aep->version |= v << 8;
				break;
			case 3:
				aep->version |= v << 16;
				break;
			case 4:
				aep->version |= v << 24;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
					    "version ok, asking for vendor\n");
				v = AEPC_VENDOR;
				if (write(aep->fd, &v, 1) != 1)
					return -1;
	        		aep->state++;
				aep->counter = 0;
				break;
			default:
				break;
			}
			break;

		case APP_INIT_VENDOR:
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
					       "vendor ok, asking for rate\n");
				v = AEPC_RATE;
				if (write(aep->fd, &v, 1) != 1)
					return -1;
	        		aep->state++;
				aep->counter = 0;
				aep->name[63] = '\0';
				break;
			default:
				aep->name[aep->counter - 2] = v;
				break;
			}
			break;


		case APP_INIT_RATE:
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 1:
				aep->rate = v;
				break;
			case 2:
				aep->rate |= v << 8;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
						    "rate ok, doing config\n");
	        		aep->state++;
				aep->counter = 0;
				cmd[1] = 0;
				if (write(aep->fd, &cmd[0], sizeof cmd) !=
								    sizeof cmd)
					return -1;
				break;
			default:
				break;
			}
			break;

		case APP_INIT_CONFIG_1:
			
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
						    "rate ok, doing config\n");
	        		aep->state++;
				aep->counter = 0;
				cmd[1] = 1;
				if (write(aep->fd, &cmd[0], sizeof cmd) !=
								    sizeof cmd)
					return -1;
				break;
			default:
				break;
			}
			break;

		case APP_INIT_CONFIG_2:
			
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
						    "rate ok, doing config\n");
	        		aep->state++;
				aep->counter = 0;
				cmd[1] = 2;
				if (write(aep->fd, &cmd[0], sizeof cmd) !=
								    sizeof cmd)
					return -1;
				break;
			default:
				break;
			}
			break;



		case APP_INIT_CONFIG_3:
			
			switch (aep->counter++) {
			case 0:
				if (v != 0xac)
					return -1;
				break;
			case 63:
				if (aep->aep_context->verbose)
					fprintf(stderr,
						"Configured new probe instance "
						"%s %s %08X %d\n",
						aep->dev_filepath, aep->name,
						      aep->version, aep->rate);

				aep->done_config |= 2;
				aep->state = APP_INIT_START_ACK;
				aep->counter = 0;
				break;
			default:
				break;
			}
			break;

		case APP_INIT_START_ACK:
			if (v != 0xac)
				return -1;

				if (aep->aep_context->verbose)
				fprintf(stderr,
					"Start acknowledge seen "
					"%s %s %08X %d\n",
					aep->dev_filepath, aep->name,
					      aep->version, aep->rate);

			aep->done_config |= 4;
			aep->state = APP_FRAME_L;
			aep->counter = 0;
			break;

		case APP_FRAME_L:
			if (v == (aep->predicted_frame & 0xff)) {
				aep->state = APP_FRAME_H;
				break;
			}
			fprintf(stderr, "%d:.L(%d %d)", aep->index, v, aep->predicted_frame & 0xff);
			/* lost sync... */
			if (aep->invalid < AEP_INVALID_COVER)
				aep->invalid = AEP_INVALID_COVER;
			aep->state = APP_FRAME_H_BAD;
			break;

		case APP_FRAME_H:

			if (v == aep->predicted_frame >> 8) {
				aep->state = APP_VOLTAGE_L1;
				aep->predicted_frame++;
				if (aep->invalid) {
					aep->invalid--;
					if (aep->invalid == 0)
						fprintf(stderr, "%d:donetime\n", aep->index);
					else
						fprintf(stderr, "-%d:%d-", aep->index, aep->invalid);
				}
				break;
			}

			fprintf(stderr, "%d:.H(%d %d)", aep->index, v, aep->predicted_frame >> 8);

		/* fallthru */

		case APP_FRAME_H_BAD:
			/* lost sync - slip one byte and use that as frame prediction for next time */
			aep->predicted_frame = v;
			if (aep->invalid < AEP_INVALID_COVER)
				aep->invalid = AEP_INVALID_COVER;
			aep->state = APP_VOLTAGE_L1_BAD;
			break;

		case APP_VOLTAGE_L1_BAD:
			aep->predicted_frame |= v << 8;
			aep->predicted_frame++;
			aep->state = APP_VOLTAGE_H1_BAD;
			break;

		case APP_VOLTAGE_H1_BAD:
		case APP_CURRENT_L1_BAD:
		case APP_CURRENT_H1_BAD:
		case APP_VOLTAGE_L2_BAD:
		case APP_VOLTAGE_H2_BAD:
		case APP_CURRENT_L2_BAD:
		case APP_CURRENT_H2_BAD:
		case APP_VOLTAGE_L3_BAD:
		case APP_VOLTAGE_H3_BAD:
		case APP_CURRENT_L3_BAD:
		case APP_CURRENT_H3_BAD:
			aep->state++;
			break;

		case APP_SWALLOW:

			//if (aep->aep_context->verbose)
				fprintf(stderr, "%s: LOST SYNC *\n",
							aep->dev_filepath);
			bad++;
			if (bad > 16) {
				fprintf(stderr, "%s: too much lost sync\n",
					aep->dev_filepath);
				return -1;
			}

			aep->state = APP_FRAME_L;
			break;

		/* good path */

		case APP_VOLTAGE_L1:
			aep->voltage = v;
			aep->state++;
			break;

		case APP_VOLTAGE_H1:
			aep->voltage |= v << 8;
			aep->state++;
			break;

		case APP_CURRENT_L1:
			aep->current = v;
			aep->state++;
			break;

		case APP_CURRENT_H1:
			aep->current |= v << 8;
			aep->state++;

			if (aep->invalid)
				break;

			if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT)
				fprintf(stderr, "\n*** %d.0: %s Shunt diff voltage %fV above"
					" %fV limit: clipping ***     \n",
					aep->index, aep->ch[0].channel_name,
					(double)aep->current / ADC_COUNTS_PER_VOLT_CH2,
					(double)ADC_COUNT_CURRENT_CLIP_LIMIT /
					ADC_COUNTS_PER_VOLT_CH2);

			return process_sample(aep, 0);
		case APP_VOLTAGE_L2:
			aep->voltage = v;
			aep->state++;
			break;

		case APP_VOLTAGE_H2:
			aep->voltage |= v << 8;
			aep->state++;
			break;

		case APP_CURRENT_L2:
			aep->current = v;
			aep->state++;
			break;

		case APP_CURRENT_H2:
			aep->current |= v << 8;
			aep->state++;

			if (aep->invalid)
				break;

			if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT)
				fprintf(stderr, "\n*** %d.1: %s Shunt diff voltage %fV above"
					" %fV limit: clipping ***     \n",
					aep->index, aep->ch[1].channel_name,
					(double)aep->current / ADC_COUNTS_PER_VOLT_CH2,
					(double)ADC_COUNT_CURRENT_CLIP_LIMIT /
					ADC_COUNTS_PER_VOLT_CH2);

			return process_sample(aep, 1);

		case APP_VOLTAGE_L3:
			aep->voltage = v;
			aep->state++;
			break;

		case APP_VOLTAGE_H3:
			aep->voltage |= v << 8;
			aep->state++;
			break;

		case APP_CURRENT_L3:
			aep->current = v;
			aep->state++;
			break;

		case APP_CURRENT_H3:
			aep->current |= v << 8;
			aep->state = APP_FRAME_L;

			if (aep->invalid) {
				fprintf(stderr, "#%d ", aep->index);
				break;
			} //else
				//fprintf(stderr, "*%d ", aep->index);

			if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT)
				fprintf(stderr, "\n*** %d.2: %s Shunt diff voltage %fV above"
					" %fV limit: clipping ***     \n",
					aep->index, aep->ch[2].channel_name,
					(double)aep->current / ADC_COUNTS_PER_VOLT_CH2,
					(double)ADC_COUNT_CURRENT_CLIP_LIMIT /
					ADC_COUNTS_PER_VOLT_CH2);

			return process_sample(aep, 2);

		}
	}

	return 0;
}

static void init_aep(struct aep_context *aep_context, struct aep *aep, const char *device_filepath)
{
	int n, m;
	struct aep_channel *ch;

	// fprintf(stderr, "***** init_aep *****\n");

	aep->head = 0;
	aep->tail = 0;
	aep->predicted_frame = 0;
	aep->state = APP_INIT_MAGIC;
	aep->invalid = 0;
	aep->counter = 0;
	aep->started = 0;
	aep->done_config = 0;

	aep->aep_context = aep_context;

	strncpy(aep->dev_filepath, device_filepath,
						sizeof aep->dev_filepath - 1);
	aep->dev_filepath[sizeof aep->dev_filepath - 1] = '\0';

	for (n = 0; n < CHANNELS_PER_PROBE; n++) {
		ch = &aep->ch[n];	

		ch->ignore = 0;
		ch->triggered = 0;
		ch->requested = 0;
		ch->summary[0] = '\0';
		ch->aep = aep;
		ch->trigger_slave = NULL;
		ch->out_head = ch->out_tail = 0;
		ch->voffset[0] = 0;
		ch->vnoise[0] = 0;
		ch->voffset[1] = 0;
		ch->vnoise[1] = 0;
		ch->rshunt = 0;
		sprintf(ch->channel_name, "%s-%d", device_filepath, n);
		ch->pretrig_ring = NULL;
		ch->ring_samples = 0;
		ch->trigger_filter = 0;
		ch->pretrigger_samples_taken = 0;
		avg_mean_us_init(&ch->avg_mean_voltage, aep->aep_context->average_len);
		avg_mean_us_init(&ch->avg_mean_current, aep->aep_context->average_len);
		ch->decimation_counter = 0;
		for (m = 0; m < sizeof(ch->min) / sizeof(ch->min[0]); m++) {
			ch->min[m] = 999;
			ch->max[m] = 0;
		}
		ch->samples_seen = 0;
		ch->simple_avg[0] = 0.0;
		ch->simple_avg[1] = 0.0;
		ch->simple_avg[2] = 0.0;
		ch->avg_count = 0;
		ch->samples = 0;
		strcpy(ch->supply, "none");
		sprintf(ch->colour, "#%06X", rand() & 0xffffff);
	}
}


static void select_map(struct aep_channel *ch)
{
	int m;
	int sel = -1;
	double delta = 999999;

	for (m = 0; m < (sizeof interp_tables / sizeof interp_tables[0]); m++) {
		if (ch->percentage_error_ref <= interp_tables[m].percentage_error_ref) {
			if (delta > (interp_tables[m].percentage_error_ref - ch->percentage_error_ref)) {
				delta = interp_tables[m].percentage_error_ref - ch->percentage_error_ref;
				sel = m;
			}
		} else {
			if (delta > (ch->percentage_error_ref - interp_tables[m].percentage_error_ref)) {
				delta = ch->percentage_error_ref - interp_tables[m].percentage_error_ref;
				sel = m;
			}
		}
	}
	if (ch->aep->aep_context->verbose)
		fprintf(stderr, "%s = corr map %d  %f%%\n",
			ch->channel_name, sel, ch->percentage_error_ref);
	ch->map_table = &interp_tables[sel];
}

void probe_close(struct aep *aep)
{
	unsigned char c = AEPC_STOP;
	int n;

	write(aep->fd, &c, 1);

	for (n = 0; n < CHANNELS_PER_PROBE; n++) {
		if (aep->ch[n].pretrig_ring)
			free(aep->ch[n].pretrig_ring);

		avg_mean_us_free(&aep->ch[n].avg_mean_voltage);
		avg_mean_us_free(&aep->ch[n].avg_mean_current);
	}

	if (aep->aep_context->verbose)
		fprintf(stderr, "closing %d\n", aep->fd);
	close(aep->fd);
}

int service_aeps(struct aep_context *aep_context, int fd_with_rx)
{
	static unsigned char zero[8] = { 0, 0, 0, 0, 0, 0, 0, 0, };
	int fd;
	int budget;
	int len;
	struct timeval tv;
	unsigned char *p;
	unsigned char c;
	char filepath[20];
	struct termios tty;
	struct serial_struct sinfo;
	int n, m, i;
	struct aep *aep;
	struct aep_channel *ch;
	unsigned char buf[AEP_INPUT_QUEUE];

	if (fd_with_rx >= 0)
		goto post_start;

	gettimeofday(&tv, NULL);

	/* look for any new devices appearing */

	if (aep_context->aeps[aep_context->scan].fd > 0)
		goto bail;

	sprintf(filepath, "/dev/ttyACM%d", aep_context->scan);
	fd = open(filepath, O_RDWR | O_NONBLOCK | O_EXCL | O_NOCTTY);

	if (fd < 0)
		goto bail;

	if (ioctl(fd, TIOCEXCL) < 0) {
		fprintf(stderr, "Wasn't able to open %s exclusive",
				filepath);
		close(fd);
		goto bail;
	}


	fprintf(stderr, "+");

	tcflush(fd, TCIOFLUSH);

	if (ioctl(fd, TIOCGSERIAL, &sinfo) == 0) {
		sinfo.closing_wait = ASYNC_CLOSING_WAIT_NONE;
		ioctl(fd, TIOCSSERIAL, &sinfo);
	}

	if (aep_context->verbose)
		fprintf(stderr, "initing %s fd=%d\n", filepath, fd);

	/* enforce suitable tty state */

	memset (&tty, 0, sizeof tty);
	if (tcgetattr (fd, &tty)) {
		fprintf(stderr, "tcgetattr failed on %s\n", filepath);
		close(fd);
		goto bail;
	}

	tty.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | XCASE |
			ECHOE | ECHOK | ECHONL | ECHOCTL | ECHOKE);
	tty.c_iflag &= ~(INLCR | IGNBRK | IGNPAR | IGNCR | ICRNL |
			   IMAXBEL | IXON | IXOFF | IXANY | IUCLC);
	tty.c_oflag &= ~(ONLCR | OPOST | OLCUC | OCRNL | ONLRET);
	tty.c_cc[VMIN]  = 1;
	tty.c_cc[VTIME] = 0;
	tty.c_cc[VEOF] = 1;
	tty.c_cflag &=  ~(CBAUD | CSIZE | CSTOPB | PARENB | CRTSCTS);
	tty.c_cflag |= (8 | CREAD | 0 | 0 | 1 | CLOCAL);

	cfsetispeed(&tty, B115200);
	cfsetospeed(&tty, B115200);
	tcsetattr(fd, TCSANOW, &tty);

	init_aep(aep_context, &aep_context->aeps[aep_context->scan], filepath);

	if (configure(aep_context, &aep_context->aeps[aep_context->scan], filepath,
					  aep_context->config_filepath, NULL) < 0) {
		fprintf(stderr, "config for %s failed\n", filepath);
		close(fd);
		goto bail;
	}

	if (write(fd, zero, sizeof(zero)) != sizeof(zero)) {
		close(fd);
		goto bail;
	}
	if (aep_context->verbose)
		fprintf(stderr, "sending reset\n");
	c = AEPC_RESET;
	if (write(fd, &c, 1) != 1) {
		close(fd);
		goto bail;
	}

	if (aep_context->verbose)
		fprintf(stderr, "done reset\n");

	aep_context->aeps[aep_context->scan].fd = fd;
	aep_context->aeps[aep_context->scan].sec_last_traffic = tv.tv_sec;
	aep_context->aeps[aep_context->scan].index = aep_context->scan;

	if (aep_context->scan > aep_context->highest)
		aep_context->highest = aep_context->scan;

	aep_context->samples(AEPSA_ADD_POLLFD, &aep_context->aeps[aep_context->scan].ch[0], fd, POLLIN | POLLERR);

	for (n = 0; n < CHANNELS_PER_PROBE; n++) {
		struct aep_channel *ch = &aep_context->aeps[aep_context->scan].ch[n];

		select_map(ch);

		if (aep_context->original_count_channel_names) {
			for (m = 0; m < aep_context->count_channel_names; m++)
				if (!strcmp(ch->channel_name, aep_context->channel_name[m]))
					break;

			if (m == aep_context->count_channel_names)
				continue;
		} else
			if (aep_context->count_channel_names < (aep_context->matched_channel_names + 1))
				aep_context->count_channel_names++;

		ch->requested = 1;
		aep_context->matched_channel_names++;

		aep_context->awaiting_capture |= 7 << (aep_context->scan * 3);
	}

	aep_context->aeps[aep_context->scan].done_config |= 1;

bail:
	aep_context->scans++;
	aep_context->scan++;
	if (aep_context->scan == MAX_PROBES)
		aep_context->scan = 0;

	/* if we're waiting to start but saw every channel / probe */

//	fprintf(stderr, "* %d %d %d\n", aep_context->has_started, aep_context->count_channel_names, aep_context->matched_channel_names);

	if (!aep_context->has_started &&
			aep_context->count_channel_names == aep_context->matched_channel_names) {
		/* nobody is waiting for config completion? */

		c = 0;
		for (m = 0; m <= aep_context->highest; m++)
			if (aep_context->aeps[m].fd > 0) {
//					fprintf(stderr, "a %d: %d\n",
//						m, aep_context->aeps[m].done_config);
				c = 1;
				if (aep_context->aeps[m].done_config != 3 || aep_context->aeps[m].invalid) {
//						fprintf(stderr, "  done_config is %d\n", aep_context->aeps[m].done_config);
					goto post_start;
				}
			}

		if (c == 0)
			goto post_start;

		/* 
		 * in the case we want to start all probes, don't know how many there are
		 * allow time to discover and recover from any initial comms error
		 */

		if (aep_context->scans < (MAX_PROBES * 2))
			goto post_start;

		/* 
		 * the number of guys we would start
		 * is all of them, is it?  Becuase we need to
		 * eliminate cross-channel latency as far as we
		 * can
		 */

		c = 0;
		for (m = 0; m <= aep_context->highest; m++) {
//				fprintf(stderr, "%d: %d %d\n", m, aep_context->aeps[m].fd, aep_context->aeps[m].done_config);
			if (aep_context->aeps[m].fd > 0 && aep_context->aeps[m].done_config == 3) {
				for (i = 0; i < CHANNELS_PER_PROBE; i++)
					if (aep_context->aeps[m].ch[i].requested)
						c++;
			}
		}

		if (c != aep_context->matched_channel_names) {
//				fprintf(stderr, "c=%d mcn=%d\n", c, aep_context->matched_channel_names);
			goto post_start;
		}

		/* tell everyone to start */

		fprintf(stderr, "Starting...\n");

		aep_context->samples(AEPSA_DATA_STARTING, NULL, 0, 0);

		for (m = 0; m <= aep_context->highest; m++) {
			if (aep_context->aeps[m].fd <= 0 ||
					aep_context->aeps[m].done_config != 3 ) {
				fprintf(stderr, "not starting %d fd %d done_config %d\n", m, aep_context->aeps[m].fd, aep_context->aeps[m].done_config);
				continue;
			}

			if (aep_context->verbose)
				fprintf(stderr, "sending start to %d\n", m);
			c = AEPC_START;
			if (write(aep_context->aeps[m].fd, &c, 1) != 1) {
				fprintf(stderr,
				     "Failed to send start\n");
			}

			for (i = 0; i < CHANNELS_PER_PROBE; i++) {
				ch = &aep_context->aeps[m].ch[i];

				if (!ch->requested)
					continue;

				aep_context->samples(AEPSA_DATA_STARTING_CHANNELS, ch, 0, 0);
			}

			aep_context->aeps[m].started = 1;
			aep_context->has_started = 1;
			aep_context->aeps[m].sec_last_traffic = tv.tv_sec;
		}

		aep_context->samples(AEPSA_DATA_STARTING_DONE, NULL, 0, 0);
	}
post_start:

	if (fd_with_rx < 0)
		goto service;

	/* somebody had something for us */

	for (m = 0; m <= aep_context->highest; m++) {

		aep = &aep_context->aeps[m];

		if (aep->fd < 1)
			continue;

		/* check for timeout */

		if (tv.tv_sec > aep->sec_last_traffic + 2) {
//				fprintf(stderr, "%d: %d %d\n", m, aep_context->aeps[m].done_config, aep_context->aeps[m].started);
			if (aep->done_config != 3 || aep->started) {
				fprintf(stderr, "%s: timeout\n", aep->dev_filepath);
				goto died;
			}
		}

		if (aep->fd != fd_with_rx)
			continue;

		if (aep_context->verbose)
			fprintf(stderr,"stuff for %d\n", m);

		aep->sec_last_traffic = tv.tv_sec;

		/* work out how much of the ring we can fill */

		if (aep->head <= aep->tail) {
			budget = (aep->head - 1) + (sizeof(aep->ring) - aep->tail);
			p = aep->ring + aep->tail;
			if (!budget)
				continue;

			len = read(aep->fd, buf, budget);
			if (len <= 0) {
				fprintf(stderr, "failed to read data\n");
				goto died;
			}

			if (aep_context->verbose)
				fprintf(stderr, "%d (a) fetched %d (budget %d)  head=%d tail=%d\n", m, len, budget, aep->head, aep->tail);

			n = sizeof(aep->ring) - aep->tail;
			if (len < n)
				n = len;

			memcpy(p, buf, n);
			aep->tail += n;
			if (aep->tail == sizeof(aep->ring))
				aep->tail = 0;

			len -= n;
			if (len) {
				memcpy(aep->ring, &buf[n], len);
				aep->tail += len;
			}

		} else {
			budget = aep->head - aep->tail - 1;
			p = aep->ring + aep->tail;
			if (!budget)
				continue;

			len = read(aep->fd, p, budget);
			if (len <= 0) {
				fprintf(stderr, "failed to read data\n");
				goto died;
			}

			if (aep_context->verbose)
				fprintf(stderr, "%d (b) fetched %d (budget %d) head=%d tail=%d\n", m, len, budget, aep->head, aep->tail);

			aep->tail += len;
		}

		if (aep->tail == sizeof(aep->ring))
			aep->tail = 0;

		continue;

died:
		fprintf(stderr, "removing probe due to error...\n");
		for (i = 0; i < CHANNELS_PER_PROBE; i++)
			if (aep->ch[i].requested) {
				aep_context->awaiting_capture &= ~(1 << (m * 3 + i));
				aep_context->matched_channel_names--;
			}
		probe_close(aep);
		aep_context->samples(AEPSA_REMOVE_POLLFD, &aep->ch[0], aep->fd, 0);
		aep->fd = 0;
		m = aep_context->highest;
	}

service:

	/* 
	 * service the existing devices
	 * need to limit the amount of time spent in the service
	 * otherwise other probes will drop data
	 */

	for (n = 0; n <= aep_context->highest; n++) {

		aep = &aep_context->aeps[n];

		if (aep->fd <= 0)
			continue;
		m = service_aep_buffers(aep, MAX_BYTES_PER_AEP_SERVICE);
		if (m >= 0)
			continue;
		if (m < -1) {
			n = aep_context->highest + 1;
			continue;
		}

		if (!aep_context->verbose)
			continue;
		fprintf(stderr, "service failed\n");
		for (i = 0; i < CHANNELS_PER_PROBE; i++) {
			if (!aep->ch[i].requested)
				continue;
			aep_context->awaiting_capture &= ~(1 << (n * 3 + i));
			aep_context->matched_channel_names--;
		}
		probe_close(aep);
		aep_context->samples(AEPSA_REMOVE_POLLFD, &aep->ch[0], aep->fd, 0);
		aep->fd = 0;
	}

	if (aep_context->scans > (5 * MAX_PROBES) &&
			aep_context->awaiting_capture == 0 && aep_context->exit_after_capture) {
		fprintf(stderr, "done all capture\n");

		return -1;
	}

	/* if nothing waiting fail immediately if anything has buffered data to service */

	aep_context->poll_timeout_ms = 10;
	for (n = 0; n <= aep_context->highest; n++) {
		if (aep_context->aeps[n].fd <= 0)
			continue;

		if (aep_context->aeps[n].head != aep_context->aeps[n].tail)
			aep_context->poll_timeout_ms = 0;
	}

	return 0;
}