#include "globals.h"
#include "i2c.h"
#include "device-functions.h"
#include "lcd.h"
#include "monitor.h"
#include "error_utils.h"
#include "menus.h"

#include <glib.h>
#include <math.h>


long monitor_last_forced_trig[max_channels];


void I2C_Setup_Monitor();
void I2C_Setup_Monitor()
{
	int i;
	float new_os;
	float average_os;
	float integrated_error;
	int channel;
	int total;

	total=0;
	average_os = 0.0;

	for (channel=0; channel<(globals.Flash.ChanKey_Curr_Mon_value?globals.Flash.channels:1); ++channel)
		if (globals.Flash.monitor_enabled[channel]) {
			++total;

			LCD_clear();
			LCD_write(0,0,"Nulling Current Monitor ....");
			/* quiescent */
			I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);
			I2C_Write(PCF8574A+Curr_Mon_LSB+channel*2,0xff);

			/* remove chip-select */
			I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xbf);
			/* pulse CAL line low */
			I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0x9f);
			I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xbf);

			for(i=0; i<50; i++);
			/* quiescent */
			I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);


			/* wait for monitor output to become stable */
			g_usleep(1e6);
			do {
				globals.ChannelState[channel].Curr_Mon_offset=0;
				integrated_error=0;

				/* repeat until stable for 50 iterations */
				int j=0;
				while (j<50) {
					g_usleep(1e4);

					/* pulse S/H line low for initial sample of monitor */
					I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0x7f);
					for (i=0; i<100; ++i) {
						;
					}
					/* quiescent */
					I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);

					/* inactive, weak pull-up mode */
					I2C_Read(PCF8574A+Curr_Mon_MSB+channel*2);

					new_os = 1.0 * I2C_Get_Monitor_Word(channel);
					average_os = (average_os*j+new_os) / (j+1.0);
					integrated_error += (average_os-new_os);

					if ( fabs(new_os-average_os) < 300.0) {
						++j;
					} else {
						j=0;
					}
				}
			} while (fabs(integrated_error) > 25.0);

			globals.ChannelState[channel].Curr_Mon_offset=(int) average_os;

			LCD_write(0,0,"Current Monitor is now ready.");
		}

	if (total) {
		I2C_Check_Monitors();
	}

	return;
}


void force_monitor_cal(int channel, int do_calibration);
void force_monitor_cal(int channel, int do_calibration)
{
	if (do_calibration) {
		/* quiescent */
		I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);
		I2C_Write(PCF8574A+Curr_Mon_LSB+channel*2,0xff);

		/* remove chip-select */
		I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xbf);

		/* pulse CAL line low */
		I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0x9f);
		I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xbf);

		g_usleep(2e3);
	}

	/* pulse trigger line low and remove chip-select to force an A/D conversion */
	I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0x3f);

	/* quiescent */
	I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);
}


int I2C_Get_Monitor_Word(int channel);
int I2C_Get_Monitor_Word(int channel)
{
	int monitor_word;

	/* remove chip-select, so that the value doesn't change while read */
	I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xbf);

	monitor_word=((I2C_Read(PCF8574A+Curr_Mon_MSB+channel*2) & 0x1f)<<8)
	             + I2C_Read(PCF8574A+Curr_Mon_LSB+channel*2);

	/* restore chip-select */
	I2C_Write(PCF8574A+Curr_Mon_MSB+channel*2,0xff);

	/* PCB 116B and earlier had 13-bit ADC, PCB 116C has 12 bit. */
	if (globals.Flash.pcb116c_mon == 1) {
		monitor_word = (monitor_word & 0x0fff) << 1;
	}

	/* deal with sign bit */
	if (monitor_word & 0x1000) {
		monitor_word=-(  ((~monitor_word)+1)  & 0x0fff);
	}

	monitor_word*=-1;

	monitor_word-=globals.ChannelState[channel].Curr_Mon_offset;

	return monitor_word;
}


int I2C_Check_Monitors(void)
{
	int monitor_word;
	float new_val, avg_val, step_size, max_dev_in_steps;
	int channel;
	int ampl_range,point_found,UseNegData,entry,word_out,atten_range;

	int update_display;
	int use_neg_data;
	int actual_pol;
	int i;
	int error_num;
	long timer_check;

	long seconds_since_last;

	update_display=0;


	for (channel=0; channel<(globals.Flash.ChanKey_Curr_Mon_value?globals.Flash.channels:1); ++channel) {
		timer_check = sec_timer();
		seconds_since_last = timer_check - monitor_last_forced_trig[channel];

		if (seconds_since_last > 0) {
			if (	(globals.ChannelState[channel].func_mode==dc_mode_on)
			                || (globals.ChannelState[channel].output_state==output_off)) {
				/* force monitor measurement every second in DC mode (because no TTL
				   trigger is present) and output-off mode */
				force_monitor_cal(channel,NO);
			}
			monitor_last_forced_trig[channel]=timer_check;
		}

		if (globals.Flash.sep_posneg_mon_ratio[channel] && globals.ChannelState[channel].amplitude<0.0) {
			use_neg_data=1;
		} else {
			use_neg_data=0;
		}

		monitor_word=I2C_Get_Monitor_Word(channel);

		/* determine current ampl_range */
		Set_VI_Control(pwl_ampl_values,channel,globals.ChannelState[channel].amplitude,&point_found,
		               &ampl_range,&atten_range,&UseNegData,&entry,&word_out,&actual_pol,NULL);

		/* 5.0V is the full-scale voltage of the 12bit ADC */
		new_val = ((((float) monitor_word)/4095.0) * 5.0)
		                / globals.Flash.mon_vi_ratio[channel][ampl_range][use_neg_data];

		/* averaging */
		for (i=CURR_MON_MAX_OLD_COUNTS-1;i>0;i--) {
			globals.ChannelState[channel].old_mon_vals[i]=globals.ChannelState[channel].old_mon_vals[i-1];
		}
		globals.ChannelState[channel].old_mon_vals[0] = new_val;

		if (globals.ChannelState[channel].num_mon_vals<CURR_MON_MAX_OLD_COUNTS) {
			++globals.ChannelState[channel].num_mon_vals;
		}

		avg_val = 0.0;
		for (i=0;i<globals.ChannelState[channel].num_mon_vals;i++) {
			avg_val += globals.ChannelState[channel].old_mon_vals[i] / globals.ChannelState[channel].num_mon_vals;
		}

		step_size=globals.Flash.monitor_step[channel];

		/* reset average if change is bigger than DEFAULT_MAX_STEP_DEV_FROM_AVG steps */

		max_dev_in_steps = MAX(globals.Flash.max_mon_steps_before_reset_average[channel], DEFAULT_MAX_STEP_DEV_FROM_AVG);
		if (fabs((new_val-avg_val)/step_size) > max_dev_in_steps) {
			globals.ChannelState[channel].num_mon_vals = 1;
			avg_val = new_val;
		}

		/* convert to even step size */
		globals.ChannelState[channel].Curr_Mon_value= ((float) ((long) (avg_val/step_size)) ) * step_size;

		/* AVO-8D2: check for duty cycle problems, as a function of measured amplitude */
		if ( globals.Flash.hard_current_limit_enabled[channel] &&
		                (globals.ChannelState[channel].output_state == output_on)) {
			for (i=0; i<max_channels; ++i) {
				TestState[i]=globals.ChannelState[i];
			}
			TestState[channel].amplitude=globals.ChannelState[channel].Curr_Mon_value;
			if ((error_num=Error_check(TestState))) {
				Set_Output_State(channel,output_off);
				queue_and_broadcast_sensor_alarm(error_num);
			}
		}

		if (globals.Flash.soft_current_limit_enabled[channel] &&
		                (globals.Flash.fully_programmed != Being_Programmed) &&
		                (globals.ChannelState[channel].Curr_Mon_value > globals.ChannelState[channel].soft_current_limit)) {
			Set_Output_State(channel,output_off);
			queue_and_broadcast_sensor_alarm(Soft_Limit_Exceeded);
		} else if (globals.MenuStatus.Type_Of_Menu==Main_Menu_On &&
		                !globals.MenuStatus.Error_Screen &&
		                !globals.MenuStatus.Nonstd_Display &&
		                globals.ChannelState[channel].Curr_Mon_value!=globals.ChannelState[channel].displayed_mon_val)
		{
			++update_display;
		}

	}

	if (update_display) {
		Show_Main_Menu();
	}

	return OK;
}