/*
 * Author: Harold Hall (haroldhhall@gmail.com)
 * creation date: 12/7/2017
 */
#include <xv_frmbufwr.h>
#include <xv_frmbufrd.h>

#include <sleep.h>
#include <xil_printf.h>

#include "servoPWM.h"

XV_frmbufwr myBuf;
XV_frmbufrd youBuf;
#define xregoff 0x14
#define yregoff 0x1c
//XPAR_IMAGE_FILTER_0_S_AXI_CONTROL_BUS_BASEADDR
int main()
{
	xil_printf("foo\n");
	//XV_frmbufwr myBuf;
	XV_frmbufwr_Initialize(&myBuf,XPAR_V_FRMBUF_WR_0_DEVICE_ID);

	XV_frmbufwr_Set_HwReg_width(&myBuf,640);
	XV_frmbufwr_Set_HwReg_height(&myBuf, 480);
	XV_frmbufwr_Set_HwReg_stride(&myBuf, 1920);
	XV_frmbufwr_Set_HwReg_video_format(&myBuf, 20);
	XV_frmbufwr_Set_HwReg_frm_buffer_V(&myBuf, XPAR_PS7_DDR_0_S_AXI_BASEADDR+0x20000);
	XV_frmbufwr_EnableAutoRestart(&myBuf);
	XV_frmbufwr_Start(&myBuf);



	//XV_frmbufrd youBuf;
	XV_frmbufrd_Initialize(&youBuf,XPAR_V_FRMBUF_RD_0_DEVICE_ID);

	XV_frmbufrd_Set_HwReg_width(&youBuf,640);
	XV_frmbufrd_Set_HwReg_height(&youBuf, 480);
	XV_frmbufrd_Set_HwReg_stride(&youBuf, 1920);
	XV_frmbufrd_Set_HwReg_video_format(&youBuf, 20);
	XV_frmbufrd_Set_HwReg_frm_buffer_V(&youBuf, XPAR_PS7_DDR_0_S_AXI_BASEADDR+0x20000);
	XV_frmbufrd_EnableAutoRestart(&youBuf);
	XV_frmbufrd_Start(&youBuf);

	PWM_Freq_Duty(PWM_X_BASE,CLK_PERIOD,SERVO_FREQ,Duty_val(90.0));
	PWM_START(PWM_X_BASE);
	PWM_Freq_Duty(PWM_Z_BASE,CLK_PERIOD,SERVO_FREQ,Duty_val(90.0));
	PWM_START(PWM_Z_BASE);

	while(1){
		static float positionX = 90.0;
		static float deltaX = 1.0;
		static float positionY = 90.0; //derp. y means z, mkay?
		static float deltaY = 1.3;
		u32 x,y;
		x = Xil_In32(XPAR_IMAGE_FILTER_0_S_AXI_CONTROL_BUS_BASEADDR+xregoff);
		y = Xil_In32(XPAR_IMAGE_FILTER_0_S_AXI_CONTROL_BUS_BASEADDR+yregoff);
		xil_printf("x=%d\ty=%d\n",x,y);
		usleep(100000);

		positionX+=deltaX;
		if (positionX >= 90.0)
			deltaX = -1.0;
		else if (positionX <= 45.0)
			deltaX = 1.0;
		positionY+=deltaY;
		if (positionY >= 125.0)
			deltaY = -1.3;
		else if (positionY <= 55.0)
			deltaY = 1.3;
		PWM_Freq_Duty(PWM_X_BASE,CLK_PERIOD,SERVO_FREQ,Duty_val(positionX));
		PWM_Freq_Duty(PWM_Z_BASE,CLK_PERIOD,SERVO_FREQ,Duty_val(positionY));
	}
	return 0;
}