/*
 * Copyright (c) 2015,
 * National Instruments Corporation.
 * All rights reserved.
 */

#include <stdio.h>
#include <time.h>
#include "MyRio.h"


// global session variable, defined in myRIO.c
extern NiFpga_Session myrio_session;

/**
 * Write a voltage value to a single AIO pin.
 *
 * @param[in]  valReg      the pin's value register to write to
 * @param[in]  goReg       the pin's go register
 * @param[in]  weight   the pin's weight value
 * @param[in]  offset   the pin's offset value
 * @param[in]  isSigned	   whether or not the pin takes a signed voltage
 * @param[in]  value       the voltage value in volts
 */
void analogWrite(uint32_t valReg, uint32_t goReg, uint32_t weight,
		uint32_t offset, NiFpga_Bool isSigned, double value) {

	// compute scaled weight and offset
	double scaledWeight = weight / 1000000000.0;
	double scaledOffset = offset / 1000000000.0;

	uint16_t outputValue;

	// convert voltage to appropriate register value using weight and offset
	if (isSigned) {
		value = (value - scaledOffset) / scaledWeight;
		value = (value < INT16_MIN) ? INT16_MIN : value;
		value = (value > INT16_MAX) ? INT16_MAX : value;

		// round the scaled value to the nearest integer
		value += (value < 0.0) ? -0.5 : 0.5;

		outputValue = (uint16_t)((int16_t)(value));
	} else {
		value = (value - scaledOffset) / scaledWeight + 0.5;
		value = (value < 0) ? 0 : value;
		value = (value > UINT16_MAX) ? UINT16_MAX : value;
		outputValue = (uint16_t) value;
	}

	NiFpga_Status status;

	// write the value to the value register
    status = NiFpga_WriteU16(myrio_session, valReg, outputValue);
    MyRio_ReturnIfNotSuccess(status, "Error writing to valReg");

    // write to the GO register to actually output the value
    status = NiFpga_WriteU16(myrio_session, goReg, 1);
    MyRio_ReturnIfNotSuccess(status, "Error writing to goReg");
}

void delay(time_t seconds) {
	time_t currentTime;
	time_t finalTime;
	time(&currentTime);
	finalTime = currentTime + seconds;
	while (currentTime < finalTime) time(&currentTime);
}


// blinks LED0 off and on
int main(int argc, char **argv)
{
	 // don't buffer stdout (required for logging to work)
	setbuf(stdout, NULL);


	printf("Air bearing test\n");

    NiFpga_Status status;

    /*
     * Open the myRIO NiFpga Session.
     * This function MUST be called before all other functions. After this call
     * is complete the myRIO target will be ready to be used.
     */
    status = MyRio_Open();
    if (MyRio_IsNotSuccess(status))
    {
    	printf("Failed to initialize NiFpga session\n");
        return status;
    }

    while (1) {
    	printf("Turning on top right\n");
    	analogWrite(AOA_0VAL, AOSYSGO, AOA_0WGHT, AOA_0OFST, NiFpga_False, 0.3);

    	printf("Turning on top left\n");
    	analogWrite(AOA_1VAL, AOSYSGO, AOA_1WGHT, AOA_1OFST, NiFpga_False, 0.3);

    	printf("Turning on bottom left\n");
    	analogWrite(AOB_0VAL, AOSYSGO, AOB_0WGHT, AOB_0OFST, NiFpga_False, 0.3);

    	printf("Turning on bottom right\n");
    	analogWrite(AOB_1VAL, AOSYSGO, AOB_1WGHT, AOB_1OFST, NiFpga_False, 0.3);

    	printf("Waiting...\n");

    	delay(10);

    	printf("Turning off top right\n");
		analogWrite(AOA_0VAL, AOSYSGO, AOA_0WGHT, AOA_0OFST, NiFpga_False, 0);

		printf("Turning off top left\n");
		analogWrite(AOA_1VAL, AOSYSGO, AOA_1WGHT, AOA_1OFST, NiFpga_False, 0);

		printf("Turning off bottom left\n");
		analogWrite(AOB_0VAL, AOSYSGO, AOB_0WGHT, AOB_0OFST, NiFpga_False, 0);

		printf("Turning off bottom right\n");
		analogWrite(AOB_1VAL, AOSYSGO, AOB_1WGHT, AOB_1OFST, NiFpga_False, 0);

		delay(10);
    }
}