///////////////////////////////////////
/// 640x480 version!
/// test VGA with hardware video input copy to VGA
// gcc with_custom_vga_driver.c -o with_custom_vga_driver -lm
///////////////////////////////////////
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/ipc.h> 
#include <sys/shm.h> 
#include <sys/mman.h>
#include <sys/time.h> 
#include "address_map_arm_brl4.h"

// the light weight bus base
void *h2p_lw_virtual_base;
volatile unsigned int *h2p_lw_video_in_control_addr=NULL;
volatile unsigned int *h2p_lw_video_in_resolution_addr=NULL;

volatile unsigned int *h2p_lw_video_edge_control_addr=NULL;

//Declare Pointers for output PIO ports
volatile unsigned int *shift_amount_ptr=NULL;
volatile unsigned int *y_video_offset_ptr=NULL;
volatile unsigned int *y_video_offset_dos_ptr=NULL;
volatile unsigned int *video_width_ptr=NULL;
volatile unsigned int *video_height_ptr=NULL;
volatile unsigned int *group_size_ptr=NULL;
volatile unsigned int *max_dist_sq_ptr=NULL;


volatile unsigned int *dpio_i_ptr=NULL;
volatile unsigned int *dpio_ii_ptr=NULL;
volatile unsigned int *dpio_iii_ptr=NULL;
volatile unsigned int *dpio_iv_ptr=NULL;
volatile unsigned int *dpio_v_ptr=NULL;
volatile unsigned int *dpio_vi_ptr=NULL;
volatile unsigned int *dpio_vii_ptr=NULL;
volatile unsigned int *dpio_viii_ptr=NULL;
volatile unsigned int *dpio_ix_ptr=NULL;

//PIO Port Offsets from Qsys
#define SHIFT_AMOUNT_OFFSET 0x10
#define Y_VIDEO_OFFSET 		0x30
#define VIDEO_WIDTH_OFFSET  0x60
#define VIDEO_HEIGHT_OFFSET 0x70
#define Y_VIDEO_OFFSET_DOS  0x80
#define GROUP_SIZE_OFFSET   0x90
#define MAX_DIST_SQ_OFFSET  0xa0

#define DPIO_I_OFFSET 		0xb0
#define DPIO_II_OFFSET 		0xc0
#define DPIO_III_OFFSET  	0xd0
#define DPIO_IV_OFFSET 		0xe0
#define DPIO_V_OFFSET  		0xf0
#define DPIO_VI_OFFSET   	0x100
#define DPIO_VII_OFFSET  	0x110
#define DPIO_VIII_OFFSET   	0x120
#define DPIO_IX_OFFSET  	0x130


// /dev/mem file id
int fd;

// shared memory 
key_t mem_key=0xf0;
int shared_mem_id; 
int *shared_ptr;
int shared_time;
int shared_note;
char shared_str[64];

int main(void)
{
	// Declare volatile pointers to I/O registers (volatile 	// means that IO load and store instructions will be used 	// to access these pointer locations, 
	// instead of regular memory loads and stores) 
  	
	// === need to mmap: =======================
	// FPGA_CHAR_BASE
	// FPGA_ONCHIP_BASE      
	// HW_REGS_BASE        
  
	// === get FPGA addresses ==================
    // Open /dev/mem
	if( ( fd = open( "/dev/mem", ( O_RDWR | O_SYNC ) ) ) == -1 ) 	{
		printf( "ERROR: could not open \"/dev/mem\"...\n" );
		return( 1 );
	}
    
    // get virtual addr that maps to physical
	h2p_lw_virtual_base = mmap( NULL, HW_REGS_SPAN, ( PROT_READ | PROT_WRITE ), MAP_SHARED, fd, HW_REGS_BASE );	
	if( h2p_lw_virtual_base == MAP_FAILED ) {
		printf( "ERROR: mmap1() failed...\n" );
		close( fd );
		return(1);
	}
    h2p_lw_video_in_control_addr=(volatile unsigned int *)(h2p_lw_virtual_base+VIDEO_IN_BASE+0x0c);
	h2p_lw_video_in_resolution_addr=(volatile unsigned int *)(h2p_lw_virtual_base+VIDEO_IN_BASE+0x08);
	*(h2p_lw_video_in_control_addr) = 0x04 ; // turn on video capture
	*(h2p_lw_video_in_resolution_addr) = 0x00f00140 ;  // high 240 low 320
	h2p_lw_video_edge_control_addr=(volatile unsigned int *)(h2p_lw_virtual_base+VIDEO_IN_BASE+0x10);
	*h2p_lw_video_edge_control_addr = 0x01 ; // 1 means edges
	*h2p_lw_video_edge_control_addr = 0x00 ; // 1 means edges
	
	//Form pointer addresses from lightweight bus base address and PIO port offset
	shift_amount_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+SHIFT_AMOUNT_OFFSET);
	y_video_offset_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+Y_VIDEO_OFFSET);
	y_video_offset_dos_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+Y_VIDEO_OFFSET_DOS);
	video_height_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+VIDEO_HEIGHT_OFFSET);
	video_width_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+VIDEO_WIDTH_OFFSET);
	group_size_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+GROUP_SIZE_OFFSET);
	max_dist_sq_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+MAX_DIST_SQ_OFFSET);
	
	
	dpio_i_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_I_OFFSET);
	dpio_ii_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_II_OFFSET);
	dpio_iii_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_III_OFFSET);
	dpio_iv_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_IV_OFFSET);
	dpio_v_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_V_OFFSET);
	dpio_vi_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_VI_OFFSET);
	dpio_vii_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_VII_OFFSET);
	dpio_viii_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_VIII_OFFSET);
	dpio_ix_ptr=(volatile unsigned int *)(h2p_lw_virtual_base+DPIO_IX_OFFSET);
	
	// video input index
	int i,j;
	
    //default values for PIO pointers
	*(shift_amount_ptr) = 5;
	*(y_video_offset_ptr) = 20;
	*(y_video_offset_dos_ptr) = 14;
	*(video_height_ptr) = 100;
	*(video_width_ptr) = 165;
	*(max_dist_sq_ptr) = 30;
	*(group_size_ptr) = 9;
	
	*(dpio_i_ptr) = 1;
	*(dpio_ii_ptr) = 10;
	*(dpio_iii_ptr) = 20;
	*(dpio_iv_ptr) = 30;
	*(dpio_v_ptr) = 40;
	*(dpio_vi_ptr) = 50;
	*(dpio_vii_ptr) = 60;
	*(dpio_viii_ptr) = 70;
	*(dpio_ix_ptr) = 80;
	
	//int and char to store values from Standard-In
	int input;
	int input2;
	char c_input;
	while(1) 
	{
		//Prompt user for input to determine PIO to set
		printf("\nCommand (s(NOP(e)),y (y shift),d(y shift dos), \n w(video width),h(video height)),g(group size),m(max dist sq), n(numIterations): ");
		scanf(" %c", &c_input);
		switch(c_input){//main switch statement
			case 's'://unused in final version of the verilog code
				printf("enter shift amount \n");
				scanf(" %d", &input);
				*(shift_amount_ptr) = input;
				break;
			case 'y'://y shift amount
				printf("enter y shift amount \n");
				scanf(" %d", &input);
				*(y_video_offset_ptr) = input;
				break;
			case 'd'://y shift amount for second image
				printf("enter y dos shift amount \n");
				scanf(" %d", &input);
				*(y_video_offset_dos_ptr) = input;
				break;
			case 'w'://Video width
				printf("enter video width val \n");
				scanf(" %d", &input);
				*(video_width_ptr) = input;
				break;
			case 'h'://video height
				printf("enter video height val \n");
				scanf(" %d", &input);
				*(video_height_ptr) = input;
				break;
			case 'g'://group size for depth map
				printf("enter group size \n");
				scanf(" %d", &input);
				*(group_size_ptr) = input;
				break;
			case 'm'://max distance squared for depth map comparison
				printf("enter max distance squared (for depth map)\n");
				scanf(" %d", &input);
				*(max_dist_sq_ptr) = input;
				break;
			case 'n'://nested swith statement to set all 9 color ranges on the depth map
				printf("Enter 1-9 for color to change\n");
				scanf("%d", &input2);
				printf("Enter depth for the color\n");
				scanf("%d", &input);
				switch(input2){
					case 1:
						*(dpio_i_ptr)= input;
						break;
					case 2:
						*(dpio_ii_ptr)= input;
						break;
					case 3:
						*(dpio_iii_ptr)= input;
						break;
					case 4:
						*(dpio_iv_ptr)= input;
						break;
					case 5:
						*(dpio_v_ptr)= input;
						break;
					case 6:
						*(dpio_vi_ptr)= input;
						break;
					case 7:
						*(dpio_vii_ptr)= input;
						break;
					case 8:
						*(dpio_viii_ptr)= input;
						break;
					case 9:
						*(dpio_ix_ptr)= input;
						break;
					default:
						printf("Number not 1-9\n");
						break;
				}
				break;
			
			default:
				printf("Invalid command\n");
				break;
		}
		
		
	} // end while(1)
} // end main

/// /// ///////////////////////////////////// 
/// end /////////////////////////////////////