// CS 4495/7495 Computer Vision
// Jim Rehg
//
// Problem Set 3
// Dynamic Programming for Stereo Correspondence
// 
// Simple implementation of dynamic programming for matching two
// scanlines with ordering constraints. The primary differences
// between this and a "real" implementation are: (1) We match single
// pixel values rather than considering an SSD window around each
// match point, (2) We consider all possible disparity values for each
// pixel rather than a limited range.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "DP.h"

// Initialize the boundary conditions for the lattice
void Lattice::initBoundary()
{
	// YOUR CODE HERE !!
}

// Make forward pass, walking over lattice and filling in cost and
// parent values by updating from neighbors above and to left

void Lattice::forwardPass(int leftscan[SCANLEN], int rightscan[SCANLEN])
{

	for(int i=1;i<TBLSIZE;i++) {
		for(int j=1;j<TBLSIZE;j++) {
			// Cost of a match
			tbl[i][j].setMin(tbl[i-1][j-1].cost + fabs(leftscan[j-1] - rightscan[i-1]),
				Cell::Matched);
			// Cost of occlusion
			tbl[i][j].updateMin(tbl[i][j-1].cost + OCCLUDE_COST, Cell::Occluded);
			// Cost of disocclusion
			// YOUR CODE HERE !!

		}
	}
}

// Start at terminal state and chain backwards through parents
// to find optimal path. Save this path into pathmap matrix and
// return the optimal cost

double Lattice::backChain(char pathmap[TBLSIZE][TBLSIZE])
{
	int i=TBLSIZE-1, j=TBLSIZE-1;
	memset((void *)pathmap, '.', TBLSIZE*TBLSIZE);
	double cost = tbl[i][j].cost;
	char id[3] = {'M', 'O', 'D'}; // Convert parent state into a character code

	while (tbl[i][j].parent > Cell::Initialized && i>0 && j>0) {
		pathmap[i][j] = id[tbl[i][j].parent];
		switch(tbl[i][j].parent) {
		case Cell::Matched:
			i = i - 1;
			j = j - 1;
			break;
		case Cell::Occluded:
			// YOUR CODE HERE !!
			break;
		case Cell::Disoccluded:
			// YOUR CODE HERE !!
			break;
		}
	}
	ASSERT(i == 0 || j == 0); // No initialized parents in center of table
	return cost;
}

// Copy the code below into some Viper callback or compile it as a
// standalone program with appropriate settings.

int main(int argc, char *argv[])
{
 
        // Pixel values for left and right scanlines
	int left[SCANLEN] = {1, 2, 10, 10, 3, 4, 5, 6, 7};
	int right[SCANLEN] = {1, 2, 3, 4, 5, 10, 10, 6, 7};
	char tblmap[TBLSIZE][TBLSIZE]; // Table for printing optimal path

	Lattice grid;
	grid.initBoundary();
	grid.forwardPass(left, right);
	double score = grid.backChain(tblmap);

        // Print out the optimal score and the character array in tblmap that
	// shows the optimal path. Turn in your print out.
	// Below is a fragment of (not very sophisticated) print code,
	// or you can use 'LogTxt' function in Viper.

	fprintf(fp, "Score: %f\n", score);
	for(int i=0; i<TBLSIZE; i++) {
		for(int j=0; j<TBLSIZE; j++) {
			fprintf(fp, "%c ", tblmap[i][j]);
		}
		fprintf(fp, "\n");
	}
	fclose(fp);
}