///////////////////////////////////////////////////////////////////////////////////////////////
// SINEIO
///////////////////////////////////////////////////////////////////////////////////////////////

#include <time.h>
#include "../myWin826.h"
#include "../826api.h"
#include "../RealTime.h"

#define SAMPLE_RATE  1000    // SET ME

// IO card configuration constants
#define IO_BOARD_NUM  0						  // no change
#define ADC_SLOT      0						  // no change

#define DAC_ZERO_OUTPUT 0x8000
#define DAC_CONFIG_GAIN S826_DAC_SPAN_10_10   // -10 to 10 spans 20 volts so...
#define DAC_VRANGE    20.0                    // This MUST correspond to the DAC_SPAN just above
#define DAC_CNT_RANGE 0xFFFF				  // 16-bit DAC
#define DAC_OFFSET_COUNTS 0x8000              // 32768 offset for a signed desired output mapped to unsigned DAC write.

#define DAC_CHANNEL   7						  // SET ME! CHECK YOUR SETUP: DAC channel output

#define ADC_CHANNEL  1                        // SET ME! CHECK YOUR SETUP: analog input channel to track


#define ADC_CNT_RANGE 0xFFFF				  // 2^16= 0xFFFF (16 bit converter) 
#define ADC_GAIN     S826_ADC_GAIN_1          // -10 to 10 option
#define ADC_VRANGE   20                       // this must correspond to gain setting


#define ADC_ENABLE 1

int main()
{
	int  errcode = S826_ERR_OK;
	int  boardflags = S826_SystemOpen();        // open 826 driver and find all 826 boards
	int  slotdata;               // adc data from the slot of interest
	int  ncount = 0;
	uint dacout;

	double voltagein = 0.0;
	double voltageout = 0;

	double Kp = 0.008;                     // Proportional gain [cite: 111]
	int current_position = 0;              // Actual position from encoder
	int ref_position = 0;                  // Desired position
	int error = 0;                         // Control error
	char key = '0';                          // Keyboard input

	// --- Data Logging Variables ---
	int data_log[2000];                    // Integer array for 2 seconds of data [cite: 111, 112]
	int log_index = 0;

	// instantiate our "real time" object that will pace our loop
	RealTime realTime(SAMPLE_RATE);

	// Configure data achqisition interfaces and start them running.
	X826(S826_AdcSlotConfigWrite(IO_BOARD_NUM, ADC_SLOT, ADC_CHANNEL, 0, ADC_GAIN));  // program adc timeslot attributes: slot, chan, 0us settling time. For -5V-5V use: "S826_ADC_GAIN_2"
	X826(S826_AdcSlotlistWrite(IO_BOARD_NUM, 1 << ADC_SLOT, S826_BITWRITE));  // enable adc timeslot; disable all other slots
	X826(S826_AdcEnableWrite(IO_BOARD_NUM, ADC_ENABLE));  // enable adc conversions

	X826(S826_DacRangeWrite(IO_BOARD_NUM, DAC_CHANNEL, DAC_CONFIG_GAIN, 0));  // program dac output range: -10V to +10V

	double Kp = 0.008;                     // Proportional gain [cite: 111]
    int current_position = 0;              // Actual position from encoder
    int ref_position = 0;                  // Desired position
    int error = 0;                         // Control error
    char key = 0;                          // Keyboard input

    // --- Data Logging Variables ---
    int data_log[2000];                    // Integer array for 2 seconds of data [cite: 111, 112]
    int log_index = 0;

	uint counts = 0;
	ref_position = current_position + 500;

	// commence pseudo-real-time loop.
	realTime.Start();  

	while (key != 'q')
	{
		// 1. Read Keyboard Input
		if (_kbhit()) // if you hit a key keyboard then read input [cite: 125]
		{
			key = _getch();
			if (key == 'M') ref_position += 500;       // 'right arrow key' increments ref [cite: 128, 132]
			else if (key == 'K') ref_position -= 500;  // 'left arrow key' decrements ref [cite: 129, 133]
		}

		if (log_index < 2000) {
			data_log[log_index] = current_position;
			log_index++;
		}

		error = ref_position - current_position;
		voltageout = Kp * error;

		//Saftey
		if (voltageout > 10.0) voltageout = 10.0;
		if (voltageout < -10.0) voltageout = -10.0;

		realTime.Sleep();
	}

	realTime.Stop(log_index); // stop real-time loop


	X826(S826_DacDataWrite(IO_BOARD_NUM, DAC_CHANNEL, DAC_ZERO_OUTPUT, 0));   // put to zero at the end
	S826_SystemClose();


	return 0;
}