Revision 1:8b8a77b7d715, committed 2016-05-03

Comitter:

wsquan171

Date:

Tue May 03 08:52:52 2016 +0000

Parent:

0:d3bf1776d1c6

Commit message:

Final demo for doing gauge needle detection on F429.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Tue May 03 08:52:52 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/mbed/code/SDFileSystem/#7b35d1709458

--- a/main.cpp	Sun May 01 19:12:13 2016 +0000
+++ b/main.cpp	Tue May 03 08:52:52 2016 +0000
@@ -1,113 +1,1177 @@
 #include "mbed.h"
 #include "SDRAM_DISCO_F429ZI.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include "SDFileSystem.h"
+
+#define M_PI           3.14159265358979323846
 
 SDRAM_DISCO_F429ZI sdram;
-
+SDFileSystem sd(PC_12, PC_11, PC_10, PB_2, "sd");// MOSI, MISO, SCK, CS
 DigitalOut led_green(LED1);
 DigitalOut led_red(LED2);
-
+SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
+DigitalOut cs(PB_2);
 Serial pc(USBTX, USBRX);
 
-#define BUFFER_SIZE         ((uint32_t)0x0100)
-#define WRITE_READ_ADDR     ((uint32_t)0x0800)
+#define line_limit 10 // limit on # of lines to be returned by HoughLines
+
+
+typedef struct
+{
+    int x;
+    int y;
+} ptvec;
+
+typedef struct
+{
+    float rho, theta;
+    double a, b;
+    double x0, y0;
+    ptvec ptvec1, ptvec2;
+    unsigned int strength;
+} trans_line;
+
+typedef struct
+{
+    unsigned int rows;
+    unsigned int cols;
+    unsigned int channels;
+    uint32_t data;
+} Image;
+
+typedef struct
+{
+    int rho;
+    int theta;
+    unsigned int strength;
+} line_polar;
+
 
-void FillBuffer(uint32_t *pBuffer, uint32_t BufferLength, uint32_t Offset);
-uint8_t CompareBuffer(uint32_t* pBuffer1, uint32_t* pBuffer2, uint16_t BufferLength);
+int bmpread(char *filename, Image *dst)
+{
+    pc.printf("Reading Image file: %s\r\n", filename);
+    FILE *file;
+    unsigned char tempSize4[4];
+    unsigned int width, height;
+    unsigned short planes, bpp;
+    
+    if ((file = fopen(filename, "rb"))==NULL)
+    {
+        pc.printf("File Not Found : %s\r\n", filename);
+        return 0;
+    }
+    
+    // seek through the bmp header, up to the width/height:
+    fseek(file, 18, SEEK_CUR);
+    if ( fread( tempSize4, 1, 4, file) < 4 )
+        return 0;
+    width = (tempSize4[3]<<24) | (tempSize4[2]<<16) | (tempSize4[1]<<8) | tempSize4[0]; // big endian
+    if ( fread( tempSize4, 1, 4, file) < 4 )
+        return 0;
+    height = (tempSize4[3]<<24) | (tempSize4[2]<<16) | (tempSize4[1]<<8) | tempSize4[0];;
+    
+    pc.printf("width: %d, height: %d\r\n", width, height);
+    
+    dst->rows = height;
+    dst->cols = width;
+    
+    
+    if ( fread( tempSize4, 1, 2, file) < 2 )
+        return 0;
+    planes = (tempSize4[1]<<8) | tempSize4[0]; // big endian
+    dst->channels = planes;
+    
+    if ( fread( tempSize4, 1, 2, file) < 2 )
+        return 0;
+    bpp = (tempSize4[1]<<8) | tempSize4[0]; // big endian
+    pc.printf("planes: %d, bpp: %d\r\n", planes, bpp);
+    
+    fseek(file, 24, SEEK_CUR);
+    unsigned char *readline;
+    unsigned int padding=0; while ((width*3+padding) % 4!=0) padding++;
+    unsigned int linewidth = width * 3 + padding;
+    readline = (unsigned char*)malloc(linewidth * sizeof(unsigned char));
+    unsigned char R, G, B;
+    int i, j;
+    
+    dst->data = 1;
+    unsigned char *dst_buf= (unsigned char *) malloc(width * sizeof(unsigned char));
+    uint32_t buffer[width/4];
+    
+    pc.printf("Converting to grayscale and Storing to SDRAM\r\n");
+    for(i = height-1; i >= 0; i--)
+    {
+        fread(readline, sizeof(unsigned char), linewidth, file);
+        for(j = 0; j < width; j++)
+        {
+            B = readline[3*j];
+            G = readline[3*j+1];
+            R = readline[3*j+2];
+            dst_buf[j] =  (0.299*R) + (0.587*G) + (0.114*B);
+        }
+        for(j = 0; j < width/4; j++)
+        {
+            buffer[j] = (16777216*dst_buf[j*4]) + (65536*dst_buf[j*4+1]) + (dst_buf[j*4+2]*256) + dst_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + dst->data + (i*width), buffer, width/4);
+        j = height / 10;
+        if(i%j == 0)
+            pc.printf("%d%%...",(100 - (i/j*10)));
+    }
+    pc.printf("\r\nImage loaded\r\n");
+    free(readline);
+    free(dst_buf);
+    fclose(file);
+    
+    return 1;
+}
+
+int Round(double number)
+{
+    double diff_down, diff_up;
+    diff_down = number - (int)number;
+    diff_up = 1 - diff_down;
+    if(diff_down < diff_up)
+        return (int)number;
+    else
+        return 1+(int)number;
+}
 
-int main()
+void matrix_convolution(Image *src, Image *dst, double **mask, int width)
 {
-    uint32_t WriteBuffer[BUFFER_SIZE];
-    uint32_t ReadBuffer[BUFFER_SIZE];
-    FMC_SDRAM_CommandTypeDef SDRAMCommandStructure;
-  
-    pc.printf("\n\nSDRAM demo started\n");
-    led_red = 0;
+    unsigned char lines[width][src->cols];
+    unsigned char result[src->cols];
+    uint32_t buffer[src->cols/4];
+    int i, j, k, r, c, actual_r, actual_c, scope;
+    double gaussian_sum;
+    //int flag = 1;
+    scope = width/2;
+    for(i = 0; i < src->rows; i++)
+    {
+        if(i < scope || (src->rows - i <= scope))
+        {
+            sdram.ReadData(i*(src->cols) + SDRAM_DEVICE_ADDR + (src->data), buffer, src->cols/4);
+            sdram.WriteData(i*(src->cols) + SDRAM_DEVICE_ADDR + (dst->data), buffer, src->cols/4);
+            continue;
+        }
+        for(j = 0; j < width; j++)
+        {
+            sdram.ReadData((i-scope+j)*(src->cols) + SDRAM_DEVICE_ADDR + (src->data), buffer, src->cols/4);
+            for(k = 0; k < src->cols/4; k++)
+            {
+                lines[j][k*4+3] = buffer[k];
+                lines[j][k*4+2] = buffer[k] >> 8;
+                lines[j][k*4+1] = buffer[k] >> 16;
+                lines[j][k*4]   = buffer[k] >> 24;
+            }
+        }
+        //pc.printf("line %d, data = %d\r\n", i, (int)lines[scope][0]);
+        
+        for(j = 0; j < src->cols; j++)
+        {
+            if( (j < scope) || (src->cols - j <= scope))
+            {
+                result[j] = lines[scope][j];
+                continue;
+            }
+            gaussian_sum = 0;
+            for(r = 0; r < width; r++)
+            {
+                for(c = 0; c < width; c++)
+                {
+                    actual_r = r;
+                    actual_c = j + c - scope;
+                    /*
+                    if(flag == 1)
+                    {
+                        pc.printf("%d, %d mask = %.4f, value = %d\r\n", r,c,mask[r][c],(int)lines[actual_r][actual_c]);
+                    }
+                    */
+                    double op1 = mask[r][c];
+                    double op2 = (double) lines[actual_r][actual_c];
+                    double temp = op1 * op2;
+                    gaussian_sum = gaussian_sum + temp;
+                }
+            }
+            //flag = 0;
+            if(gaussian_sum > 255) gaussian_sum = 255;
+            if(gaussian_sum < 0) gaussian_sum  = 0;
+            result[j] = (unsigned char)Round(gaussian_sum);
+            //pc.printf("%d ", result[j]);
+        }
+        //pc.printf("\r\n");
+        for(j = 0; j < src->cols/4; j++)
+        {
+            buffer[j] = (16777216*result[j*4]) + (65536*result[j*4+1]) + (result[j*4+2]*256) + result[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + dst->data + (i*src->cols), buffer, src->cols/4);
+        j = src->rows / 10;
+        if(i%j == 0)
+            pc.printf("%d%%...",i/j*10);
+    }
+    pc.printf("\r\n");
+    return;
+}
+
+
+void myGaussianBlur(Image *src, Image *dst, int width, double sigma)
+{
+    // build the Gaussian Kernal
+    // ref: http://stackoverflow.com/questions/8204645/implementing-gaussian-blur-how-to-calculate-convolution-matrix-kernel
+    
+    pc.printf("\r\nGaussian Blur Module Invoked\r\n");
+    dst->rows = src->rows;
+    dst->cols = src->cols;
+    dst->channels = src->channels;
+    dst->data = src->data + (src->rows * src->cols);
+    
+    int W = width;
+    int i, x, y;
+    double **kernel;
+    kernel = (double **) malloc(W * sizeof(double *));
+    for(i = 0; i < W; i++)
+        kernel[i] = (double *) malloc(W * sizeof(double));
+    double mean = W/2;
+    double sum = 0.0; // For accumulating the kernel values
+    for (x = 0; x < W; ++x)
+    {
+        for (y = 0; y < W; ++y)
+        {
+            kernel[x][y] = exp( -0.5 * (pow((x-mean)/sigma, 2.0) + pow((y-mean)/sigma,2.0)) )
+            / (2 * M_PI * sigma * sigma);
+            
+            // Accumulate the kernel values
+            sum += kernel[x][y];
+        }
+    }
+    // Normalize the kernel
+    //pc.printf("Gaussian Kernal in use:\r\n");
+    for (x = 0; x < W; ++x)
+    {
+        for (y = 0; y < W; ++y)
+        {
+            kernel[x][y] /= sum;
+            //pc.printf("%.7f ", kernel[x][y]);
+        }
+        //pc.printf("\r\n");
+    }
+    
+    // do matrix convolution
+    matrix_convolution(src, dst, kernel, width);
+
+    
+    for(i = 0; i < W; i++)
+        free(kernel[i]);
+    free(kernel);
+    return;
+}
 
-    // Fill the write buffer
-    FillBuffer(WriteBuffer, BUFFER_SIZE, 0xA244250F);
-  
-    // Write buffer
-    sdram.WriteData(SDRAM_DEVICE_ADDR + WRITE_READ_ADDR, WriteBuffer, BUFFER_SIZE);
-    pc.printf("Write data DONE\n");
+void myCanny(Image *src, Image *dst, double min_threshold, double max_threshold)
+{
+    // ref:
+    // http://dasl.mem.drexel.edu/alumni/bGreen/www.pages.drexel.edu/_weg22/can_tut.html
+    // https://en.wikipedia.org/wiki/Canny_edge_detector
+    
+    pc.printf("\r\nCanny Edge Detection Module Invoked\r\n");
+    
+    dst->rows = src->rows;
+    dst->cols = src->cols;
+    dst->channels = src->channels;
+    dst->data = src->data + (src->rows * src->cols);
+    
+    // de-noise using local Gaussian Blur, simulating size = 5, sigma = 1.4
+    pc.printf("Smoothing image...\r\n");
+    double mask[5][5] =
+    {
+        {2,  4,  5,  4,  2},
+        {4,  9, 12,  9,  4},
+        {5, 12, 15, 12,  5},
+        {4,  9, 12,  9,  4},
+        {2,  4,  5,  4,  2}
+    };
+    double **kernel;
+    int i, j;
+    kernel = (double **) malloc(5 * sizeof(double *));
+    for(i = 0; i < 5; i++)
+        kernel[i] = (double *) malloc(5 * sizeof(double));
+    for(i = 0; i < 5; i++)
+    {
+        for(j = 0; j < 5; j++)
+        {
+            kernel[i][j] = mask[i][j] / 159;
+            //printf("%.7f ", kernel[i][j]);
+        }
+        //printf("\n");
+    }
+    matrix_convolution(src, dst, kernel, 5);
+    for(i = 0; i < 5; i++)
+        free(kernel[i]);
+    free(kernel);
+    
+    // find gradient x and gradient y
+    pc.printf("Finding gradient in x and y for each pixel...\r\n");
+    Image Gx, Gy;   // Gx and Gy are used to store x-gradient and y-gradient
+    Gx.cols = src->cols;
+    Gx.rows = src->rows;
+    Gx.data = dst->data + (dst->cols * dst->rows);
+    Gy.cols = src->cols;
+    Gy.rows = src->rows;
+    Gy.data = Gx.data + (dst->cols * dst->rows);
+    
+    // size-3 Sobel mask
+    double sobel_x[3][3] = {{-1, 0, 1}, {-2, 0, 2}, {-1, 0, 1}};
+    double sobel_y[3][3] = {{-1, -2, -1}, {0, 0, 0}, {1, 2, 1}};
+    double **mask_x, **mask_y;
+    mask_x = (double **) malloc(3 * sizeof(double *));
+    for(i = 0; i < 3; i++)
+        mask_x[i] = (double *) malloc(3 * sizeof(double));
+    mask_y = (double **) malloc(3 * sizeof(double *));
+    for(i = 0; i < 3; i++)
+        mask_y[i] = (double *) malloc(3 * sizeof(double));
+    for(i = 0; i < 3; i++)
+    {
+        for(j = 0; j < 3; j++)
+        {
+            mask_x[i][j] = sobel_x[i][j];
+            mask_y[i][j] = sobel_y[i][j];
+        }
+    }
+    pc.printf("in x: ");
+    matrix_convolution(src, &Gx, mask_x, 3);
+    pc.printf("in y: ");
+    matrix_convolution(src, &Gy, mask_y, 3);
+    for(i = 0; i < 3; i++)
+    {
+        free(mask_x[i]);
+        free(mask_y[i]);
+    }
+    free(mask_x);
+    free(mask_y);
+    //pc.printf("dst at %d, Gx at %d, Gy at %d\r\n", dst->data, Gx.data, Gy.data);
+    
+    // calculate gradient strength (store in Gx) and direction (store in Gy)
+    pc.printf("Calculating gradient strength and direction...\r\n");
+    double gx_val, gy_val;
+    double arctan_val;
+    int over_threshold_count = 0;  // number used for adaptive non-max suppression
+    uint32_t buffer[src->cols/4];
+    unsigned char Gx_buf[src->cols], Gy_buf[src->cols];
+    //pc.printf("%d %d %d %d %d %d", src->rows, src->cols, Gx.cols, Gx.rows, Gy.cols, Gy.rows);
+    for(i = 0; i < src->rows; i++)
+    {
+        sdram.ReadData(i*(Gx.cols) + SDRAM_DEVICE_ADDR + (Gx.data), buffer, Gx.cols/4);
+        for(j = 0; j < Gx.cols/4; j++)
+        {
+            Gx_buf[j*4+3] = buffer[j];
+            Gx_buf[j*4+2] = buffer[j] >> 8;
+            Gx_buf[j*4+1] = buffer[j] >> 16;
+            Gx_buf[j*4]   = buffer[j] >> 24;
+        }
+        sdram.ReadData(i*(Gy.cols) + SDRAM_DEVICE_ADDR + (Gy.data), buffer, Gy.cols/4);
+        for(j = 0; j < Gy.cols/4; j++)
+        {
+            Gy_buf[j*4+3] = buffer[j];
+            Gy_buf[j*4+2] = buffer[j] >> 8;
+            Gy_buf[j*4+1] = buffer[j] >> 16;
+            Gy_buf[j*4]   = buffer[j] >> 24;
+        }
+        
+        for(j = 0; j < src->cols; j++)
+        {
+            gx_val = Gx_buf[j];
+            gy_val = Gy_buf[j];
+            
+            Gx_buf[j] = (unsigned char)sqrt(pow(gx_val, 2) + pow(gy_val, 2));
+            if(Gx_buf[j] > max_threshold)
+                over_threshold_count++;
+            arctan_val = atan(gy_val / gx_val);
+            if(arctan_val >= 0.41421 && arctan_val < 2.414421)    // 22.5 to 67.5 deg
+                Gy_buf[j] = 45;
+            else if(arctan_val >= 2.41421 || arctan_val < -2.41421) // 67.5 to 112.5 deg
+                Gy_buf[j] = 90;
+            else if(arctan_val >= -2.41421 && arctan_val < 0.41421) // 112.5 to 157.5 deg
+                Gy_buf[j] = 135;
+            else    // 0 to 22.5 and 157.5 to 180 deg
+                Gy_buf[j] = 0;
+        }
+        
+        for(j = 0; j < Gx.cols/4; j++)
+        {
+            buffer[j] = (16777216*Gx_buf[j*4]) + (65536*Gx_buf[j*4+1]) + (Gx_buf[j*4+2]*256) + Gx_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + Gx.data + (i*Gx.cols), buffer, Gx.cols/4);
+        for(j = 0; j < Gy.cols/4; j++)
+        {
+            buffer[j] = (16777216*Gy_buf[j*4]) + (65536*Gy_buf[j*4+1]) + (Gy_buf[j*4+2]*256) + Gy_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + Gy.data + (i*Gy.cols), buffer, Gy.cols/4);
+    }
+    
+    // Non-maximum suppression. Aim is to shrink the edge width
+    pc.printf("Non-maximum suppression...\r\n");
+    int mask_radius = 1;    // hard-coded parameter
+    int r, c, not_max_flag, vertical_neighbor;
+    Image sup_matrix;      // maxtrix used to keep track of local maxima stutas for each pixel
+    sup_matrix.cols = dst->cols;
+    sup_matrix.rows = dst->rows;
+    sup_matrix.data = Gy.data + (dst->cols * dst->rows);
+    double skip_sup_ratio = (double)over_threshold_count/8800; // 10000 pixels should be found in a 800x600 pic
+    if(skip_sup_ratio < 1)
+        skip_sup_ratio = 1;
+    else if(skip_sup_ratio > 1.8)
+        skip_sup_ratio = 1.8;
+    pc.printf("Over max threshold count: %d\r\n", over_threshold_count);
+    //pc.printf("Skip suppression ratio: %.2f\r\n", skip_sup_ratio);
+    unsigned char sup_buf[dst->cols], Gx_buf_array[3][dst->cols], Gy_buf_array[3][dst->cols];
+    for(i = mask_radius; i < src->rows - mask_radius; i++)
+    {
+        sup_buf[j] = 0;   // in sup_matrix, 0 is default value
+        for(r = 0; r < 3; r++)
+        {
+            sdram.ReadData((i+r-1)*(Gx.cols) + SDRAM_DEVICE_ADDR + (Gx.data), buffer, Gx.cols/4);
+            for(j = 0; j < Gx.cols/4; j++)
+            {
+                Gx_buf_array[r][j*4+3] = buffer[j];
+                Gx_buf_array[r][j*4+2] = buffer[j] >> 8;
+                Gx_buf_array[r][j*4+1] = buffer[j] >> 16;
+                Gx_buf_array[r][j*4]   = buffer[j] >> 24;
+            }
+        }
+        for(r = 0; r < 3; r++)
+        {
+            sdram.ReadData((i+r-1)*(Gy.cols) + SDRAM_DEVICE_ADDR + (Gy.data), buffer, Gy.cols/4);
+            for(j = 0; j < Gy.cols/4; j++)
+            {
+                Gy_buf_array[r][j*4+3] = buffer[j];
+                Gy_buf_array[r][j*4+2] = buffer[j] >> 8;
+                Gy_buf_array[r][j*4+1] = buffer[j] >> 16;
+                Gy_buf_array[r][j*4]   = buffer[j] >> 24;
+            }
+        }
+    
+        for(j = mask_radius; j < src->cols - mask_radius; j++)
+        {
+            if( j < mask_radius || (j >= src->rows - mask_radius))
+            {
+                sup_buf[j] = 1;
+                continue;
+            }
+            not_max_flag = 0;
+            
+            // pixels with significant high magnitute will never be suppressed
+            if(Gx_buf_array[1][j] > 1.8 * max_threshold)
+            {
+                sup_buf[j] = 2;   // in sup matrix, 2 means maxima's
+                continue;
+            }
+            
+            // for other pixels, compare its gradient magnitute with its neighbor
+            for(r = -mask_radius; r <= mask_radius; r++)
+            {
+                if(not_max_flag == 1)
+                    break;
+                for(c = -mask_radius; c <= mask_radius; c++)
+                {
+                    vertical_neighbor = 0;
+                    // only neighbors vertical to the gradient direction will be checked
+                    switch (Gy_buf_array[1][j])
+                    {
+                        case 0:
+                            if(c == 0 && (r == -1 || r ==1))
+                                vertical_neighbor = 1;
+                            break;
+                        case 45:
+                            if((r == 1 && c == -1) || (r == -1 && c == 1))
+                                vertical_neighbor = 1;
+                            break;
+                        case 90:
+                            if(r == 0 && (c == -1 || c == 1))
+                                vertical_neighbor = 1;
+                        case 135:
+                            if((r == 1 && c == 1) || (r == -1 && c == -1))
+                                vertical_neighbor = 1;
+                        default:
+                            break;
+                    }
+                    if(vertical_neighbor != 1)
+                        continue;
+                    
+                    if(Gy_buf_array[1][j] == Gy_buf_array[1+r][j+c])
+                    {
+                        if(Gx_buf_array[1][j] < Gx_buf_array[1+r][j+c])
+                        {
+                            not_max_flag = 1;
+                            sup_buf[j] = 1;
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+        
+        for(j = 0; j < sup_matrix.cols/4; j++)
+        {
+            buffer[j] = (16777216*sup_buf[j*4]) + (65536*sup_buf[j*4+1]) + (sup_buf[j*4+2]*256) + sup_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + sup_matrix.data + (i*sup_matrix.cols), buffer, sup_matrix.cols/4);
+    }
+    
+        // Gy is no longer used
+    
+    // Double threshold
+    pc.printf("Double threshold...\r\n");
+    unsigned char dst_buf[dst->cols];
+    int a = 0, b = 0, d = 0;
+    for(i = 0; i < src->rows; i++)
+    {
+        sdram.ReadData(i*(Gx.cols) + SDRAM_DEVICE_ADDR + (Gx.data), buffer, Gx.cols/4);
+        for(j = 0; j < Gx.cols/4; j++)
+        {
+            Gx_buf[j*4+3] = buffer[j];
+            Gx_buf[j*4+2] = buffer[j] >> 8;
+            Gx_buf[j*4+1] = buffer[j] >> 16;
+            Gx_buf[j*4]   = buffer[j] >> 24;
+        }
+        sdram.ReadData(i*(sup_matrix.cols) + SDRAM_DEVICE_ADDR + (sup_matrix.data), buffer, sup_matrix.cols/4);
+        for(j = 0; j < Gx.cols/4; j++)
+        {
+            sup_buf[j*4+3] = buffer[j];
+            sup_buf[j*4+2] = buffer[j] >> 8;
+            sup_buf[j*4+1] = buffer[j] >> 16;
+            sup_buf[j*4]   = buffer[j] >> 24;
+        }
+        
+        for(j = 0; j < src->cols; j++)
+        {
+            if(i == 0 || j == 0 || i == src->rows-1 || j == src->cols-1)
+            {
+                dst_buf[j] = 0;
+                b++;
+                continue;
+            }
+            if(sup_buf[j] == 1)
+                Gx_buf[j] = 0;
+            if(Gx_buf[j] > max_threshold)   // strong edge
+            {
+                dst_buf[j] = 255;
+                a++;
+            }
+            else if(Gx_buf[j] < min_threshold)  // weak edge
+            {
+                dst_buf[j] = 0;
+                b++;
+            }
+            else    // in middle: later check if it is connected to a strong edge
+            {
+                dst_buf[j] = Gx_buf[j];
+                d++;
+            }
+        }
+        for(j = 0; j < dst->cols/4; j++)
+        {
+            buffer[j] = (16777216*dst_buf[j*4]) + (65536*dst_buf[j*4+1]) + (dst_buf[j*4+2]*256) + dst_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + dst->data + (i*dst->cols), buffer, dst->cols/4);
+    }
+    pc.printf("strong = %d, weak = %d, middle = %d\r\n", a, b, d);
+    
+    int k;
+    // Gx and sup_matrix is no longer used
+    
+    // pixel which has magnitude in-middle will be preserved if connected to a strong edge
+    pc.printf("Edge tracking by hysteresis...\r\n");
+    short min_neighbor;
+    unsigned char dst_buf_array[3][dst->cols];
+    for(i = 0; i < 10; i++)
+    {
+        for(j = 1; j < dst->rows-1; j++)
+        {
+            for(r = 0; r < 3; r++)
+            {
+                sdram.ReadData((i+r-1)*(dst->cols) + SDRAM_DEVICE_ADDR + (dst->data), buffer, dst->cols/4);
+                for(k = 0; k < dst->cols/4; k++)
+                {
+                    dst_buf_array[r][k*4+3] = buffer[k];
+                    dst_buf_array[r][k*4+2] = buffer[k] >> 8;
+                    dst_buf_array[r][k*4+1] = buffer[k] >> 16;
+                    dst_buf_array[r][k*4]   = buffer[k] >> 24;
+                }
+            }
+            for(k = 1; k < dst->cols - 1; k++)
+            {
+                if(dst_buf_array[1][j] == 0 || dst_buf_array[1][j] == 255)
+                    continue;
+                min_neighbor = 0;
+                for(r = -1; r <= 1; r++)
+                {
+                    for(c = -1; c <= 1; c++)
+                    {
+                        if(dst_buf_array[1+r][j+c] > max_threshold)
+                            dst_buf_array[1][j] = 255;
+                        else if(dst_buf_array[1+r][j+c] < min_threshold)
+                            min_neighbor++;
+                    }
+                }
+                if(min_neighbor == 8)
+                    dst_buf_array[1][j] = 0;
+            }
+            for(k = 0; k < dst->cols/4; k++)
+            {
+                buffer[j] = (16777216*dst_buf_array[1][k*4]) + (65536*dst_buf_array[1][k*4+1]) + (dst_buf_array[1][k*4+2]*256) + dst_buf_array[1][k*4+3];
+            }
+            sdram.WriteData(SDRAM_DEVICE_ADDR + dst->data + (i*dst->cols), buffer, dst->cols/4);
+        }
+    }
+    
+    // those can't reach to a strong edge in 10 pixels will be supressed
+    for(i = 0; i < src->rows; i++)
+    {
+        sdram.ReadData((i)*(dst->cols) + SDRAM_DEVICE_ADDR + (dst->data), buffer, dst->cols/4);
+        for(k = 0; k < dst->cols/4; k++)
+        {
+            dst_buf[k*4+3] = buffer[k];
+            dst_buf[k*4+2] = buffer[k] >> 8;
+            dst_buf[k*4+1] = buffer[k] >> 16;
+            dst_buf[k*4]   = buffer[k] >> 24;
+        }
+        for(j = 0; j < src->cols; j++)
+        {
+            if((dst_buf[j] == 255) || (dst_buf[j] == 0))
+                continue;
+            else
+                dst_buf[j] = 0;
+        }
+        for(j = 0; j < dst->cols/4; j++)
+        {
+            buffer[j] = (16777216*dst_buf[j*4]) + (65536*dst_buf[j*4+1]) + (dst_buf[j*4+2]*256) + dst_buf[j*4+3];
+        }
+        sdram.WriteData(SDRAM_DEVICE_ADDR + dst->data + (i*dst->cols), buffer, dst->cols/4);
+    }
+    
+    int zero = 0, max = 0, in_middle = 0;
+    for(i = 0; i < src->rows; ++i)
+    {
+
+        sdram.ReadData((i)*(dst->cols) + SDRAM_DEVICE_ADDR + (dst->data), buffer, dst->cols/4);
+        for(k = 0; k < dst->cols/4; k++)
+        {
+            dst_buf[k*4+3] = buffer[k];
+            dst_buf[k*4+2] = buffer[k] >> 8;
+            dst_buf[k*4+1] = buffer[k] >> 16;
+            dst_buf[k*4]   = buffer[k] >> 24;
+        }
+
+        for ( j = 0; j < src->cols; ++j)
+        {
+            if(dst_buf[j] == 0)
+                zero++;
+            else if(dst_buf[j] == 255)
+                max++;
+            else
+                in_middle++;
+        }
+    }
+    pc.printf("zero = %ld, max = %ld, in_middle = %ld\r\n", zero, max, in_middle);
+    pc.printf("SDRAM peak usage %ld\r\n", sup_matrix.data + (src->cols * src->rows) -1);
+    
+    return;
+}
 
-    // Issue self-refresh command to SDRAM device
-    SDRAMCommandStructure.CommandMode            = FMC_SDRAM_CMD_SELFREFRESH_MODE;
+int myHoughLines(Image *src, line_polar *line_list, int threshold, int number)
+{
+    // ref:
+    // http://www.keymolen.com/2013/05/hough-transformation-c-implementation.html
+    // https://en.wikipedia.org/wiki/Hough_transform
+    
+    pc.printf("\r\nHoughLines Module Invoked\r\n");
+    
+    // initialize the accumulator for lines. rho and theta are resolutions in distance and angle (all set to 1)
+    unsigned int dimension_r, dimension_t, i, j;
+    //if(src->channels != 1)  // only accept grayscal images
+    //    return -1;
+    if(src->rows >= src->cols)
+        dimension_r = src->rows;
+    else
+        dimension_r = src->cols;
+    dimension_r = (unsigned int) ((double)dimension_r * sqrt((double)2)) + 1;
+    dimension_t = 360;
+    
+    uint32_t error_count = 0, addr = src->data + (src->rows * src->cols);
+    pc.printf("accumulator start at %ld\r\n", addr-1);
+    uint32_t *accumulator = (uint32_t*)malloc(dimension_t*sizeof(uint32_t));
+    for(i = 0; i < dimension_r; i++)
+    {
+        for(j = 0; j < dimension_t; j++)
+            accumulator[j] = 0;
+        sdram.WriteData(SDRAM_DEVICE_ADDR + (4*i*dimension_t) + addr, accumulator, dimension_t);
+    }
+    for(i = 0; i < dimension_r;i++)
+    {
+        sdram.ReadData(SDRAM_DEVICE_ADDR + (4*i*dimension_t) + addr, accumulator, dimension_t);
+        for(j = 0; j < dimension_t; j++)
+        {
+            if(accumulator[j] != 0)
+            {
+                error_count++;
+            }
+        }
+    }
+    pc.printf("error_count = %ld\r\n", error_count);
+    
+    pc.printf("Accumulator dimension: %d(rho) by %d(theta)\r\n", dimension_r, dimension_t);
+    
+    // scan through all edge pixels.
+    int t, neg_r = 0, round_up = 0, round_down = 0, pixel_count = 0;
+    double new_x, new_y, theta_rad, r;
+    unsigned char src_buf[src->cols];
+    uint32_t buffer[src->cols/4], tmp_accu;
+    
+    for(i = 0; i < src->rows; i++)
+    {
+        sdram.ReadData(i*(src->cols) + SDRAM_DEVICE_ADDR + (src->data), buffer, src->cols/4);
+        for(j = 0; j < src->cols/4; j++)
+        {
+            src_buf[j*4+3] = buffer[j];
+            src_buf[j*4+2] = buffer[j] >> 8;
+            src_buf[j*4+1] = buffer[j] >> 16;
+            src_buf[j*4]   = buffer[j] >> 24;
+        }
+        for(j = 0; j < src->cols; j++)
+        {
+            if(src_buf[j] != 255)  // only consider edge pixels (who have value of 255)
+                continue;
+            pixel_count++;
+            for(t = 0; t < dimension_t; t++)    // for each theta, calc the corresponding rho
+            {
+                new_x = (double)i;
+                new_y = (double)j;
+                theta_rad = (double)t * (M_PI / 180);
+                r = (new_y * cos(theta_rad)) + (new_x * sin(theta_rad));
+                
+                if(r <= 0) // negative rho = positive rho, they are duplicated lines that can be discarded
+                {
+                    neg_r++;
+                    continue;
+                }
+                
+                if( r - (int)r >= 0.5)  // round up to the bin
+                {
+                    round_up++;
+                    sdram.ReadData(4*dimension_t*((int)r +1) + SDRAM_DEVICE_ADDR + (addr) + (4*t), &tmp_accu, 1);
+                    tmp_accu++;
+                    if(tmp_accu > 503640)
+                        printf("(%d, %d)\r\n", (int)r +1, t);
+                    sdram.WriteData(4*dimension_t*((int)r +1) + SDRAM_DEVICE_ADDR + (addr) + (4*t), &tmp_accu, 1);
+                }
+                else    // round down to the bin
+                {
+                    round_down++;
+                    sdram.ReadData(4*dimension_t*((int)r) + SDRAM_DEVICE_ADDR + (addr) + (4*t), &tmp_accu, 1);
+                    tmp_accu++;
+                    sdram.WriteData(4*dimension_t*((int)r) + SDRAM_DEVICE_ADDR + (addr) + (4*t), &tmp_accu, 1);
+                }
+            }
+        }
+        j = src->rows / 10;
+        if(i%j == 0)
+            pc.printf("%d%%...",i/j*10);
+    }
+    pc.printf("\r\n");
+    pc.printf("negatve rho = %d\r\nround up = %d, round down = %d\r\n", neg_r, round_up, round_down);
+    pc.printf("pixel count = %d\r\n", pixel_count);
+    // find top x lines in rho-theta space
+    int weakest = 0, count = 0, weakest_pt = 0;
+    line_polar local_line_list[number];
+    for(i = 0; i < dimension_r; i++)
+    {
+        sdram.ReadData(4*i*dimension_t + SDRAM_DEVICE_ADDR + (addr), accumulator, dimension_t);
+        for(j = 0; j < dimension_t; j++)
+        {
+            if(accumulator[j] < threshold || accumulator[j] <= weakest)
+                continue;
+            
+            local_line_list[weakest_pt].rho = i;
+            local_line_list[weakest_pt].theta = j;
+            sdram.ReadData(4*dimension_t*i + SDRAM_DEVICE_ADDR + (addr) + (4*j), &tmp_accu, 1);
+            local_line_list[weakest_pt].strength = tmp_accu;
+            count++;
+            
+            if(count < number)
+            {
+                weakest_pt++;
+                weakest = 0;
+            }
+            else
+            {
+                weakest = local_line_list[0].strength;
+                weakest_pt = 0;
+                for(t = 1; t < number; t++)
+                {
+                    if(local_line_list[t].strength < weakest)
+                    {
+                        weakest = local_line_list[t].strength;
+                        weakest_pt = t;
+                    }
+                }
+            }
+        }
+    }
+    
+    // copy those found lines baack to main's stack
+    memcpy(line_list, local_line_list, number*sizeof(line_polar));
+    
+    for(i = 0; i < number; i++)
+    {
+        if(count == i)
+            break;
+        printf("line %d: rho = %d, theta = %d, strength = %d\r\n", i, local_line_list[i].rho, local_line_list[i].theta, local_line_list[i].strength);
+    }
+    printf("\r\n");
+    free(accumulator);
+    // return the actual number of lines found above threshold
+    return count;
+}
+
+int det(ptvec a, ptvec b)
+{
+    return (a.x * b.y) - (a.y * b.x);
+}
+
+int line_intersection(trans_line line1, trans_line line2, ptvec *result)
+{
+    ptvec xdiff, ydiff, d;
+    xdiff.x = line1.ptvec1.x - line1.ptvec2.x;
+    xdiff.y = line2.ptvec1.x - line2.ptvec2.x;
+    ydiff.x = line1.ptvec1.y - line1.ptvec2.y;
+    ydiff.y = line2.ptvec1.y - line2.ptvec2.y;
+    
+    int div = det(xdiff, ydiff);
+    if(div == 0)
+    {
+        // Lines don't intersect
+        return 0;
+    }
+    
+    d.x = det(line1.ptvec1, line1.ptvec2);
+    d.y = det(line2.ptvec1, line2.ptvec2);
+    result->x = (int) det(d, xdiff) / div;
+    result->y = (int) det(d, ydiff) / div;
+    return 1;
+}
+
+double LineAnalysis(trans_line *line_list, int line_count, int dim_x, int dim_y)
+{
+    printf("Needle Position Detection Module Invoked.\r\n");
+    int strongest_pt = 0, i = 0, j = 0, upper_count = 0, lower_count = 0;
+    int upper_bin[line_count];
+    int lower_bin[line_count];
+    double average_deg = 0, diff, theta_deg;
+    
+    // find the line with highest confidence
+    for(i = 0; i < line_count; i++)
+    {
+        if(line_list[i].strength > line_list[strongest_pt].strength)
+            strongest_pt = i;
+    }
+    printf("Line used as reference of de-noise: rho = %d, ", (int)line_list[strongest_pt].rho);
+    printf("theta = %d\r\n", (int) (line_list[strongest_pt].theta * 180 / M_PI));
+    
+    // delete lines whose theta varies > 10 deg from the strongest line
+    for(i = 0; i < line_count; i++)
+    {
+        diff = 180 * line_list[strongest_pt].theta / M_PI;
+        diff = diff - (180 * line_list[i].theta / M_PI);
+        if(diff >= 10 || diff <= -10)
+        {
+            printf("Line with confidence of %d is suppressed.\r\n", line_list[i].strength);
+            line_list[i].strength = 0;
+            continue;
+        }
+        average_deg = (180 * line_list[i].theta / M_PI) + average_deg;
+        j++;
+    }
+    average_deg = average_deg / j;
+    printf("Theta used to divide lines into bins: %.2f\r\n", average_deg);
+    
+    // categorize all lines into 2 bins depends on whether its theta is higher than average.
+    for(i = 0; i < line_count; i++)
+    {
+        if(line_list[i].strength == 0)
+            continue;
+        
+        theta_deg = line_list[i].theta * 180 / M_PI;
+        if(theta_deg > average_deg)
+        {
+            upper_bin[upper_count] = i;
+            upper_count++;
+        }
+        else
+        {
+            lower_bin[lower_count] = i;
+            lower_count++;
+        }
+    }
+    
+    // choose the bin with minority of lines.
+    // find a line in the bin with highest / lowest theta if we chose upper / lower bin
+    // the chosen line is considered as the line mostly diverted from the other lines but yet near needle
+    // find all intersections between this line and the lines in the bin with majority of lines
+    // since this line is well seperated from others, points found should be far enough from gauge center
+    int ref_pt = 0, inter_count = 0;
+    ptvec intersect_list[line_count];
+    if(lower_count < upper_count)
+    {
+        for(i = 0; i < lower_count; i++)
+        {
+            if(line_list[lower_bin[i]].theta < line_list[lower_bin[ref_pt]].theta)
+                ref_pt = i;
+        }
+        average_deg = (180 * line_list[lower_bin[ref_pt]].theta / M_PI);
+        printf("Line in lower_bin is used to find intersections:\r\n");
+        printf("rho = %d, ", (int) (line_list[lower_bin[ref_pt]].rho));
+        printf("theta = %d ", (int) (line_list[lower_bin[ref_pt]].theta * 180 / (M_PI)));
+        printf("strength = %d\r\n", (int) (line_list[lower_bin[ref_pt]].strength));
+        for(i = 0; i < upper_count; i++)
+        {
+            if(line_intersection(line_list[lower_bin[ref_pt]], line_list[upper_bin[i]], &intersect_list[inter_count]) == 1)
+                inter_count++;
+        }
+        ref_pt = 0;
+        for(i = 0; i < upper_count; i++)
+        {
+            if(line_list[upper_bin[i]].theta > line_list[upper_bin[ref_pt]].theta)
+                ref_pt = i;
+        }
+        average_deg = (180 * line_list[upper_bin[ref_pt]].theta / M_PI) + average_deg;
+        average_deg = average_deg / 2;
+    }
+    else
+    {
+        for(i = 0; i < upper_count; i++)
+        {
+            if(line_list[upper_bin[i]].theta > line_list[upper_bin[ref_pt]].theta)
+                ref_pt = i;
+        }
+        average_deg = (180 * line_list[upper_bin[ref_pt]].theta / M_PI);
+        printf("Line in upper_bin is used to find intersections:\r\n");
+        printf("rho = %d, ", (int) (line_list[upper_bin[ref_pt]].rho));
+        printf("theta = %d ", (int) (line_list[upper_bin[ref_pt]].theta * 180 / (M_PI)));
+        printf("strength = %d\r\n", (int) (line_list[upper_bin[ref_pt]].strength));
+        for(i = 0; i < lower_count; i++)
+        {
+            if(line_intersection(line_list[upper_bin[ref_pt]], line_list[lower_bin[i]], &intersect_list[inter_count]) == 1)
+                inter_count++;
+        }
+        ref_pt = 0;
+        for(i = 0; i < lower_count; i++)
+        {
+            if(line_list[lower_bin[i]].theta < line_list[lower_bin[ref_pt]].theta)
+                ref_pt = i;
+        }
+        average_deg = (180 * line_list[lower_bin[ref_pt]].theta / M_PI) + average_deg;
+        average_deg = average_deg / 2;
+    }
+    
+    // categorize all intersections points into 4 bins
+    //  ----------------> x
+    //  |       |
+    //  |  III  |   IV
+    //  |       |
+    //  |---------------
+    //  |       |
+    //  |  II   |   I
+    //  v       |
+    //  y
+    // the reason is that the center of the pic should be close to the center of the gauge.
+    // the angle of the needle is already detected. we don't need actually find the center
+    // to tell the direction of the needle. the distribution of intersections can provide
+    // enough info to do the desicion.
+    
+    int cor_count[4] = {0, 0, 0, 0};
+    int var_x = 0, var_y = 0;
+    printf("Intersections:\r\n");
+    for(i = 0; i < inter_count; i++)
+    {
+        if(intersect_list[i].x > dim_x /2)
+        {
+            if(intersect_list[i].y > dim_y/2)
+                cor_count[0]++;
+            else
+                cor_count[3]++;
+        }
+        else
+        {
+            if(intersect_list[i].y > dim_y/2)
+                cor_count[1]++;
+            else
+                cor_count[2]++;
+        }
+        var_x = var_x + abs(intersect_list[i].x - dim_x/2);
+        var_y = var_y + abs(intersect_list[i].y - dim_y/2);
+        printf("(%d, %d)\r\n", intersect_list[i].x, intersect_list[i].y);
+    }
+    printf("Variation in x: %d, in y: %d\r\n", var_x, var_y);
+    
+    int most_cor_pt = 0;
+    printf("Intersection points distribution: \r\n");
+    for(i = 0; i < 4; i++)
+    {
+        if(cor_count[i] > cor_count[most_cor_pt])
+            most_cor_pt = i;
+        printf("Cor %d: %d\r\n", i+1, cor_count[i]);
+    }
+    
+    int correct_vote = 0, revert_vote = 0;
+    if(average_deg >= 0 && average_deg < 90)    // intersections are expected to be in II or IV
+    {
+        if(var_x < var_y)   // variance in x is smaller, I+II should > III+IV
+        {
+            if((cor_count[0]+cor_count[1]) > (cor_count[2]+cor_count[3]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        else            // variance in y is smaller, II+III should > IV+I
+        {
+            if((cor_count[1]+cor_count[2]) > (cor_count[3]+cor_count[0]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        if(correct_vote > revert_vote)
+            printf("Original detected angle is correct.\r\n");
+        else
+        {
+            average_deg = average_deg + 180;
+            printf("Reverted angle is correct.\r\n");
+        }
+    }
+    else if(average_deg >= 90 && average_deg < 180)    // intersections are expected to be in III or I
+    {
+        if(var_x < var_y)   // variance in x is smaller, I+II should < III+IV
+        {
+            if((cor_count[0]+cor_count[1]) < (cor_count[2]+cor_count[3]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        else            // variance in y is smaller, II+III should > IV+I
+        {
+            if((cor_count[1]+cor_count[2]) > (cor_count[3]+cor_count[0]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        if(correct_vote > revert_vote)
+            printf("Original detected angle is correct.\r\n");
+        else
+        {
+            average_deg = average_deg + 180;
+            printf("Reverted angle is correct.\r\n");
+        }
+    }
+    else if(average_deg >= 180 && average_deg < 270)    // intersections are expected to be in II or IV
+    {
+        if(var_x < var_y)   // variance in x is smaller, I+II should < III+IV
+        {
+            if((cor_count[0]+cor_count[1]) < (cor_count[2]+cor_count[3]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        else            // variance in y is smaller, II+III should < IV+I
+        {
+            if((cor_count[1]+cor_count[2]) < (cor_count[3]+cor_count[0]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        if(correct_vote > revert_vote)
+            printf("Original detected angle is correct.\r\n");
+        else
+        {
+            average_deg = average_deg - 180;
+            printf("Reverted angle is correct.\r\n");
+        }
+    }
+    else if(average_deg >= 270 && average_deg < 360)    // intersections are expected to be in III or I
+    {
+        if(var_x < var_y)   // variance in x is smaller, I+II should > III+IV
+        {
+            if((cor_count[0]+cor_count[1]) > (cor_count[2]+cor_count[3]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        else            // variance in y is smaller, II+III should < IV+I
+        {
+            if((cor_count[1]+cor_count[2]) < (cor_count[3]+cor_count[0]))
+                correct_vote++;
+            else
+                revert_vote++;
+        }
+        if(correct_vote > revert_vote)
+            printf("Original detected angle is correct.\r\n");
+        else
+        {
+            average_deg = average_deg - 180;
+            printf("Reverted angle is correct.\r\n");
+        }
+    }
+    else
+    {
+        printf("Detection failed. The result may not be correct.\r\n");
+    }
+    return average_deg;
+}
+
+
+int main(void)
+{
+    pc.baud(115200);
+    int i, j;
+    led_red = 0;
+    led_green = 0;
+    
+    FMC_SDRAM_CommandTypeDef SDRAMCommandStructure;
     SDRAMCommandStructure.CommandTarget          = FMC_SDRAM_CMD_TARGET_BANK2;
-    SDRAMCommandStructure.AutoRefreshNumber      = 1;
-    SDRAMCommandStructure.ModeRegisterDefinition = 0;
-    if (sdram.Sendcmd(&SDRAMCommandStructure) != HAL_OK) 
+
+    pc.printf("\r\nSDRAM demo started\r\n");
+    
+    Image src, dst, bin;
+    if (bmpread("/sd/1.bmp", &src) == 0)
+        return 0;
+    
+    // smooth the picture using GaussianBlur: void GaussianBlur(Image *src, Image *dst, int width, double sigma)
+    myGaussianBlur(&src, &dst, 5, 1.5);
+    myCanny(&dst, &bin, 20, 60);
+    line_polar my_line_list[10];
+    int line_count;
+    line_count = myHoughLines(&bin, my_line_list, 40, line_limit);
+    if(line_count >= line_limit)
+        line_count = line_limit;
+        
+    trans_line line_list[line_limit];
+    unsigned int m = src.rows, n = src.cols;
+    for(i = 0; i < line_count; i++)  // combined for loops at #64, #73, #83, #92, #101
     {
-      led_red = 1;
-      error("BSP_SDRAM_Sendcmd FAILED\n");
-    }
-  
-    // SDRAM memory read back access
-    SDRAMCommandStructure.CommandMode = FMC_SDRAM_CMD_NORMAL_MODE;
-    if (sdram.Sendcmd(&SDRAMCommandStructure) != HAL_OK) 
-    {
-      led_red = 1;
-      error("BSP_SDRAM_Sendcmd FAILED\n"); 
+        line_list[i].rho = my_line_list[i].rho;
+        line_list[i].theta = my_line_list[i].theta * (M_PI / 180);
+        line_list[i].strength = my_line_list[i].strength;
+        line_list[i].a = cos(line_list[i].theta);
+        line_list[i].b = sin(line_list[i].theta);
+        line_list[i].x0 = line_list[i].a * line_list[i].rho;
+        line_list[i].y0 = line_list[i].b * line_list[i].rho;
+        line_list[i].ptvec1.x = Round(line_list[i].x0 + (m+n)*(-line_list[i].b));
+        line_list[i].ptvec1.y = Round(line_list[i].y0 + (m+n)*line_list[i].a);
+        line_list[i].ptvec2.x = Round(line_list[i].x0 - (m+n)*(-line_list[i].b));
+        line_list[i].ptvec2.y = Round(line_list[i].y0 - (m+n)*line_list[i].a);
     }
 
-    while(1) {
-      
-      // Read back data from the SDRAM memory
-      sdram.ReadData(SDRAM_DEVICE_ADDR + WRITE_READ_ADDR, ReadBuffer, BUFFER_SIZE); 
-      pc.printf("\nRead data DONE\n");
-
-      // Checking data integrity 
-      if (CompareBuffer(WriteBuffer, ReadBuffer, BUFFER_SIZE) != 0)
-      {
-        led_red = !led_red;
-        pc.printf("Write/Read buffers are different\n");
-      }
-      else
-      {
-        led_green = !led_green;
-        pc.printf("Write/Read buffers are identical\n");
-      }
-  
-      wait(1);
-    }
+    double angle = LineAnalysis(line_list, line_count, src.cols, src.rows);
+    double value = (angle - 42) / 270 * 50;
+    pc.printf("Detected angle = %.2f\r\nEstimated value = %.2f\r\n", angle, value);
 }
-
-/**
-  * @brief  Fills buffer with user predefined data.
-  * @param  pBuffer: pointer on the buffer to fill
-  * @param  BufferLength: size of the buffer to fill
-  * @param  Value: first value to fill on the buffer
-  * @retval None
-  */
-void FillBuffer(uint32_t *pBuffer, uint32_t BufferLength, uint32_t Value)
-{
-  uint32_t tmpIndex = 0;
-
-  /* Put in global buffer different values */
-  for (tmpIndex = 0; tmpIndex < BufferLength; tmpIndex++ )
-  {
-    pBuffer[tmpIndex] = tmpIndex + Value;
-  }
-} 
-
-/**
-  * @brief  Compares two buffers.
-  * @param  pBuffer1, pBuffer2: buffers to be compared.
-  * @param  BufferLength: buffer's length
-  * @retval 0: pBuffer2 identical to pBuffer1
-  *         1: pBuffer2 differs from pBuffer1
-  */
-uint8_t CompareBuffer(uint32_t* pBuffer1, uint32_t* pBuffer2, uint16_t BufferLength)
-{
-  while (BufferLength--)
-  {
-    if (*pBuffer1 != *pBuffer2)
-    {
-      return 1;
-    }
-
-    pBuffer1++;
-    pBuffer2++;
-  }
-
-  return 0;
-}