//#define _SUPRESS_PLIB_WAEEING 1
#include <plib.h>
// Configuration Bit settings
// System Clock = 60 MHz, Peripherial Bus = 7.5MHz
// Primary Osc w/PLL (XT+,HS+,EC+PLL)
// Input Divider 2x Divider
// Multiplier 15x Multiplier
//
//#define CCLK(60000000) //8Mhz Osc on Explorer16 board (pll 8 / 2 * 15)
//#define PBCLK (CCLK/8)
//#define Fsck375000
//#define BRG_VAL (PBCLK/2/Fsck)
#include "config.h"
#include "pt_cornell_1_2.h"
#include <math.h>

////////////////////////////////////
// graphics libraries
#include "tft_master.h"
#include "tft_gfx.h"

// States for the Lock System
#define WELCOME         0
#define CAL_EMPTY       1
#define CAL_FIST        2
#define CAL_OPEN        3
#define MATCHING        4
#define RESET_PW        5
#define BUTTON_PRESS    6

// Maximum Password Length
#define PW_LENGTH       5

// Threads
static struct pt pt_control;

#define Image_col 176 // image format QCIF 176x144
#define Image_row 144 // 4 pixel per byte
int HREF,frame;
//int image[ 25344 ];
char buffer[60];
unsigned char image_test[ Image_col ][ Image_row ];
int password[PW_LENGTH] = {1,1,1,1,1};
int password_tried[PW_LENGTH];
int start = 0;

#define BRG_VAL (pb_clock / 8 )
#define Nop() asm( "nop" )

int black;
int emptPixCount = 1000;
int fistPixCount = 6000;
int openPixCount = 9700;
int emptPixRenge[2] = {0};
int fistPixRenge[2] = {0};
int openPixRenge[2] = {0};

/////////////////////////////////////////////
/*
 * takePicture( int calibrate )
 *    Utilizes the OV7670 camera module to take a snapshot of the camera. The image
 *    is store in global variable "image_test". Also, the number of pixels that
 *    are less than the threshold are store in global variable "black".
 *
 *    @PARAM  calibrate   1 if recalibrate the threshold and store unmodified picture
 *                        0 if no calibration and utilize existing threshold
 */
void takePicture(int calibrate);

/*
 * picture_display()
 *    Displays the full picture onto the TFT display. Utilizes global variable "image_test"
 *    as the input for data.
 */
void picture_display();

/*
 * writeI2C1_rex()
 *    Writes data to the I2C channel and waits for the channel to become idle.
 */
int writeI2C1_rex ( char data )
{
    MasterWriteI2C1( data ); //write address
    IdleI2C1();
}
/////////////////////////////////////////////
/*
 * i2c_wait()
 *    Writes NOPs for the PIC32 to wait for I2C channel to become idle.
 */
void i2c_wait(unsigned int cnt)
{
    while(--cnt)
    {
        Nop();
        Nop();
    }
}
/////////////////////////////////////////////

int camera_done = 0, calibration, match_mode, reset_mode, all_done = 1;

static PT_THREAD (protothread_control(struct pt *pt))
{
    PT_BEGIN(pt);
    
    int GameState = WELCOME;
    //int button_B13, button_A0;
    //int obutton_B13 = 1, obutton_A0 = 1;
    int unselected = 1;
    int move, mode=0, confirm, old_move = 0, old_confirm = 0;
    int numPixels[3];
    int i,j;
    int pw_idx;
    int check_number;
    int pass;
    
    while(1)
    {
        switch(GameState)
        {
            // WELCOME - choose what mode to use
            case WELCOME:
                tft_fillRect(0,0, 240, 320, ILI9340_RED);// x,y,w,h,color
                tft_fillRect(3,3, 234, 314, ILI9340_BLACK);// x,y,w,h,color
                //tft_fillRect(40,40,200,10,ILI9340_BLACK); 
                tft_setCursor(10,20);
                sprintf(buffer,"  Gesture Unlock\n      System");
                tft_writeString(buffer);
                tft_setCursor(20,120);
                sprintf(buffer,"Begin Matching");
                tft_writeString(buffer);
                tft_setCursor(20,160);
                sprintf(buffer,"Reset Password");
                tft_writeString(buffer);
                tft_setCursor(20,200);
                sprintf(buffer,"Camera Calibration");
                tft_writeString(buffer);
                tft_fillCircle(15, 125+mode*40, 2, ILI9340_WHITE );
                unselected = 1;
                while (unselected)  // a mode has not been selected yet
                {
                    move = mPORTAReadBits( BIT_0 );
                    if (move == 0 && old_move != 0  ) {
                        tft_fillCircle(15, 125+mode*40, 2, ILI9340_BLACK );
                        mode++;
                        mode = mode%3;
                        tft_fillCircle(15, 125+mode*40, 2, ILI9340_WHITE );
                    }
                    old_move = move;
                    confirm = mPORTBReadBits( BIT_13 );
                    if (confirm == 0 && old_confirm != 0 )
                    {
                        unselected = 1;
                        tft_fillRect(10,10, 10, 10, ILI9340_RED);
                        if(mode == 2 )
                        {
                            //calibration = 1;
                            GameState = CAL_FIST;
                            tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                            tft_setCursor(40,40);
                            sprintf(buffer,"Make a fist\n    and press any\n      button");
                            tft_writeString(buffer);                            
                            all_done = 0;
                            break;
                        }
                        else if( mode == 0)
                        {
                            //match_mode = 1;
                            GameState = MATCHING;
                            tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                            tft_setCursor(40,40);
                            sprintf(buffer,"Begin Matching");
                            tft_writeString(buffer);
                            all_done = 0;
                            //display_done = 1;
                            break;
                        }
                        else if (mode == 1)
                        {
                            //reset_mode = 1;
                            GameState = RESET_PW;
                            tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                            tft_setCursor(40,40);
                            sprintf(buffer,"Reset Password\n Select hand gesture\n and press any button");
                            tft_writeString(buffer);
                            tft_setCursor(5,160);
                            sprintf(buffer,"       Please\n  enter the old \n         password", i);
                            tft_writeString(buffer);  
                            all_done = 0;
                            
                            pw_idx = 0;
                            pass = 0;
//                            for(i = 0; i < PW_LENGTH; i++)
//                            {
//                                password[i] = 0;
//                            }
                            break;
                        }
                    }
                    old_confirm = confirm;
                }
                break;
            
            // First step of calibration - calibrate fist
            case CAL_FIST:
                //button_B13 = !mPORTBReadBits( BIT_13 );
                //button_A0  = !mPORTAReadBits( BIT_0 );
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                
                // check for button press
                if (( move == 0 && old_move != 0  )
                        || (confirm == 0 && old_confirm != 0 ))
                {
                    GameState = CAL_OPEN;
                    takePicture(1);
                    picture_display();
                    takePicture(1);
                    picture_display();
                    takePicture(0);
                    numPixels[0] = black;
                    picture_display();
                    takePicture(0);
                    numPixels[1] = black;
                    picture_display();
                    takePicture(0);
                    numPixels[2] = black;
                    picture_display();
                    if((numPixels[0] < numPixels[1])
                            && (numPixels[0] < numPixels[2]))
                    {
                        fistPixCount = numPixels[0];
                    }
                    else if((numPixels[1] < numPixels[0])
                                && (numPixels[1] < numPixels[2]))
                    {
                        fistPixCount = numPixels[1];
                    }
                    else
                    {
                        fistPixCount = numPixels[2];
                    }
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(15,40);
                    sprintf(buffer,"Open your hand\n   and press any\n     button");
                    tft_writeString(buffer);
                }
                old_move = move;
                old_confirm = confirm;
                break;
            
            // Second step of calibration - calibrate open
            case CAL_OPEN:
                //button_B13 = !mPORTBReadBits( BIT_13 );
                //button_A0  = !mPORTAReadBits( BIT_0 );
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                
                // check for button press
                if (( move == 0 && old_move != 0  )
                        || (confirm == 0 && old_confirm != 0 ))
                {
                    GameState = CAL_EMPTY;
                    
                    takePicture(0);
                    picture_display();
                    numPixels[0] = black;
                    takePicture(0);
                    picture_display();
                    numPixels[1] = black;
                    takePicture(0);
                    picture_display();
                    numPixels[2] = black;
                    if((numPixels[0] < numPixels[1])
                            && (numPixels[0] < numPixels[2]))
                    {
                        openPixCount = numPixels[0];
                    }
                    else if((numPixels[1] < numPixels[0])
                                && (numPixels[1] < numPixels[2]))
                    {
                        openPixCount = numPixels[1];
                    }
                    else
                    {
                        openPixCount = numPixels[2];
                    }
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(15,40);
                    sprintf(buffer,"Take your hand out\n    and press any\n    button");
                    tft_writeString(buffer);
                }
                old_move = move;
                old_confirm = confirm;
                break;
               
            // third and final step of calibration - check no hand
            case CAL_EMPTY:
                //button_B13 = !mPORTBReadBits( BIT_13 );
                //button_A0  = !mPORTAReadBits( BIT_0 );
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                
                // check for any button press
                if (( move == 0 && old_move != 0  )
                        || (confirm == 0 && old_confirm != 0 ))
                {
                    GameState = WELCOME;
                    
                    // find the median value
                    takePicture(0);
                    picture_display();
                    numPixels[0] = black;
                    takePicture(0);
                    picture_display();
                    numPixels[1] = black;
                    takePicture(0);
                    picture_display();
                    numPixels[2] = black;
                    if((numPixels[0] < numPixels[1])
                            && (numPixels[0] < numPixels[2]))
                    {
                        emptPixCount = numPixels[0];
                    }
                    else if((numPixels[1] < numPixels[0])
                                && (numPixels[1] < numPixels[2]))
                    {
                        emptPixCount = numPixels[1];
                    }
                    else
                    {
                        emptPixCount = numPixels[2];
                    }
                    //record the boundary values
                    emptPixRenge[0] = 0;
                    emptPixRenge[1] = emptPixCount + (fistPixCount-emptPixCount)/2;
                    fistPixRenge[0] = fistPixCount - (fistPixCount-emptPixCount)/2;
                    fistPixRenge[1] = fistPixCount + (openPixCount-fistPixCount)/2;
                    openPixRenge[0] = openPixCount - (openPixCount-fistPixCount)/2;
                    openPixRenge[1] = Image_row * Image_col;
                }
                old_move = move;
                old_confirm = confirm;
                break;
                
            // Hand Sign checking - for unlocking the system
            case MATCHING:
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                for(i = 0; i<5 ; i++)
                {
                    //takes a series of pictures to determine length
                    takePicture(0);
                    picture_display();
                    check_number = black;
                    tft_writeString(buffer);
                    if( (check_number >= emptPixRenge[0]) && (check_number <=emptPixRenge[1]))
                    {
                        //tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                        tft_setCursor(10,40);tft_setTextSize(1);
                        sprintf(buffer,"No Hand Detected!\n   Press any key to Exit...\n ");
                        tft_writeString(buffer);
                        tft_setTextSize(2);
                        GameState = BUTTON_PRESS;
                        i = 7;
                        break;
                    }
                    else if(check_number >= fistPixRenge[0] && check_number <= fistPixRenge[1])
                    {
                        password_tried[i] = 1;
                        
                    }
                    else if(check_number >= openPixRenge[0] && check_number <= openPixRenge[1])
                    {
                        password_tried[i] = 2;
                    }
                    else 
                    {
                        password_tried[i] = 3;
                    }
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(10,160);
                    sprintf(buffer,"    gesture Done\n  Move to the next \n This Gesture is: %d", password_tried[i]);
                    tft_writeString(buffer);                   
                }
                if(i != 7){                     // if not empty detected
                    for (i = 0; i < 5 ; i++)
                    {
                        if(password_tried[i] != password[i])
                        {
                            tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                            tft_setCursor(5,160);
                            sprintf(buffer,"       Failed!\n  Press any Button\n       to Exit", i);
                            tft_writeString(buffer); 
                            GameState = BUTTON_PRESS;
                            break;
                        }
                    }
                    if( i == 5 )                // if all correct
                    {
                        tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                        tft_setCursor(5,160);
                        sprintf(buffer,"       Unlocked!\n  Press any Button\n         to Exit", i);
                        tft_writeString(buffer);    
                        GameState = BUTTON_PRESS;
                    }
                }
//                if (( move == 0 && old_move != 0  )
//                        || (confirm == 0 && old_confirm != 0 ))
//                {
//                    
//                    GameState = WELCOME;
//
//                }
                old_move = move;
                old_confirm = confirm;
                break;
                
            // reset the password
            case RESET_PW:
                if(!pass)
                {
                // checks for old password
                for(i = 0; i<5 ; i++)
                {
                    takePicture(0);
                    picture_display();
                    check_number = black;
                    tft_writeString(buffer);
                    if( (check_number >= emptPixRenge[0]) && (check_number <=emptPixRenge[1]))
                    {
                        //tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                        tft_setCursor(10,40);tft_setTextSize(1);
                        sprintf(buffer,"No Hand Detected!\n   Press any key to Exit...\n ");
                        tft_writeString(buffer);
                        tft_setTextSize(2);
                        GameState = BUTTON_PRESS;
                        i = 7;
                        break;
                    }
                    else if(check_number >= fistPixRenge[0] && check_number <= fistPixRenge[1])
                    {
                        password_tried[i] = 1;
                        
                    }
                    else if(check_number >= openPixRenge[0] && check_number <= openPixRenge[1])
                    {
                        password_tried[i] = 2;
                    }
                    else 
                    {
                        password_tried[i] = 3;
                    }
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(10,160);
                    sprintf(buffer,"    gesture Done\n  Move to the next \n This Gesture is: %d", password_tried[i]);
                    tft_writeString(buffer);                   
                }
                if(i != 7){                     // if not empty detected
                    for (i = 0; i < 5 ; i++)
                    {
                        if(password_tried[i] != password[i])
                        {
                            tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                            tft_setCursor(5,160);
                            sprintf(buffer,"       Failed!\n  Press any Button\n       to Exit\n ");
                            tft_writeString(buffer); 
                            GameState = BUTTON_PRESS;
                            break;
                        }
                    }
                    if( i == 5 )                // if all correct
                    {
                        tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                        tft_setCursor(5,160);
                        sprintf(buffer,"       Unlocked!\n  enter the new\n     password", i);
                        tft_writeString(buffer); 
                        pass = 1;
                        i = 0;
                        pw_idx = 0;
                        break;
                    }
                }
                }
                
                // recording new password
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                                
                if ((confirm == 0 && old_confirm != 0)
                        || (pw_idx == PW_LENGTH) )
                {
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(5,40);
                    sprintf(buffer,"the new password is :\n %d %d %d %d %d \n  press any key\n  to continue...", password[0],password[1],
                            password[2], password[3], password[4]);
                    tft_writeString(buffer);    
                    GameState = BUTTON_PRESS;
                }
                else if (move == 0 && old_move != 0  )
                {
                    takePicture(0);
                    check_number = black;
                    picture_display();
                    if( (check_number >= emptPixRenge[0]) && (check_number <=emptPixRenge[1]))
                    {
                        password[pw_idx] = 0;
                        pw_idx++;
                    }
                    else if(check_number >= fistPixRenge[0] && check_number <= fistPixRenge[1])
                    {
                        //password_tried[i] = 1;
                        password[pw_idx] = 1;
                        pw_idx++;
                    }
                    else if(check_number >= openPixRenge[0] && check_number <= openPixRenge[1])
                    {
                        //password_tried[i] = 2;
                        password[pw_idx] = 2;
                        pw_idx++;
                    }
                    else
                    {
                        password[pw_idx] = 3;
                        pw_idx++;
                    }
                    tft_fillRect(3,3, 234, 314, ILI9340_BLACK);
                    tft_setCursor(10,160);
                    sprintf(buffer,"    gesture Done\n  Move to the next ");
                    tft_writeString(buffer);      
                }
                old_move = move;
                old_confirm = confirm;
                break;
                
            // wait stage; previous stage displays message on screen
            case BUTTON_PRESS:
                move = mPORTAReadBits( BIT_0 );
                confirm = mPORTBReadBits( BIT_13 );
                
                // waits for button press to move to the next stage
                if (( move == 0 && old_move != 0  )
                        || (confirm == 0 && old_confirm != 0 ))
                {
                    GameState = WELCOME;
                }
                old_move = move;
                old_confirm = confirm;
                break;
        }
    }
    PT_END(pt);
}


/////////////////////////////////////////////
int main(void)
{
    
    //------------ set up I2C change camera output to QCIF & YUV-------------
    unsigned char WriteSlaveAddress, ReadSlaveAddress;
    char i2cData[10];
    unsigned char i2cbyte;
    
    // Set Periph Bus Divider 40MHz / 2 = 20MHz Fpb
    mOSCSetPBDIV( OSC_PB_DIV_4 );
    //OpenI2C1( I2C_EN, 8000);
    OpenI2C1( I2C_EN, BRG_VAL );
    
    WriteSlaveAddress = 0x42;  //Serial EEPROM address
    ReadSlaveAddress  = 0x43;

    // Send Data to eeprom to program one location
    i2cData[0] = WriteSlaveAddress;
    i2cData[1] = 0x0C;      // address for enabling scaling
    i2cData[3] = 0x12;      // address for enabling QCIF mode
    i2cData[5] = 0x11;      // address for clkrc
    i2cData[7] = 0x15;      // address for VSYNC 
    //i2cData[7] = 0x6B;      // address for dblv/pll
    
    
    StartI2C1();//Send the Start Bit
    IdleI2C1();//Wait to complete
            //enabling QCIF format
    i2cData[4] = 0x08 ; //0x84;
    writeI2C1_rex( WriteSlaveAddress );
    writeI2C1_rex( i2cData[3]  );
    writeI2C1_rex( i2cData[4] );
    StopI2C1();
    IdleI2C1();
    
    StartI2C1();//Send the Start Bit
    IdleI2C1();//Wait to complete
    // clkrc - setting the clock rate
    i2cData[6] = 0x90; 
    writeI2C1_rex( WriteSlaveAddress );
    writeI2C1_rex( i2cData[5] );
    writeI2C1_rex( i2cData[6] );
    
    StopI2C1();
    IdleI2C1();

    // dblv/pll
//s
    
    
    StartI2C1();//Send the Start Bit
    IdleI2C1();//Wait to complete
    // enabling scaling for QCIF version
    i2cData[2] = 0x08;
    writeI2C1_rex( WriteSlaveAddress );
    writeI2C1_rex( i2cData[1] );
    writeI2C1_rex( i2cData[2] );
    StopI2C1();
    IdleI2C1();
    CloseI2C1();

    //----------set up done--------------
    //----------port set up--------------
    mPORTBSetPinsDigitalIn( BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7 | BIT_10 | BIT_11 | BIT_12 ); // set RB3-RB19 connect to D0-D7
    mPORTASetPinsDigitalIn( BIT_2 | BIT_4 ); // RA2 for HREF, RA4 for PCLK
    mPORTBSetPinsDigitalIn( BIT_15 ); // VSYNC changed to RB15
    mPORTBSetPinsDigitalIn( BIT_13 ); // control buttom    
    
    //setting LED (D6) on PIC32
    mPORTASetPinsDigitalOut( BIT_0 );
    mPORTASetBits( BIT_0 );
    
    frame = 0; // init frame conter, Horizental line conter;
    HREF = 0;
    
    // enable system wide interrupt
    INTEnableSystemMultiVectoredInt();
    
    PT_setup();
    
    
    // init the threads
    PT_INIT(&pt_control);
  

    tft_init_hw();
    tft_begin();
    tft_fillScreen(ILI9340_BLACK);
    tft_setTextColor(ILI9340_WHITE);tft_setTextSize(2);
    
	// print count, wait for char, print char
	while (1)
	{
        PT_SCHEDULE(protothread_control(&pt_control));
  }
}


int VSYNC = 0;
int col,row,Y,i;
int HREF = 0, oHREF = 0;
int PCLK = 0, oPCLK = 0;
unsigned char hst[255]= {0};
unsigned char temp;
int max1, max2, max3, max_value;
int thresh = 50;

int sum = 0;
int sumB = 0;
int wB = 0;
int wF = 0;
int mB, mF;
int max = 0;
int between = 0;
int threshold1 = 0;
int threshold2 = 0;
int total = Image_row * Image_col;

/*
 * takePicture( int calibrate )
 *    Utilizes the OV7670 camera module to take a snapshot of the camera. The image
 *    is store in global variable "image_test". Also, the number of pixels that
 *    are less than the threshold are store in global variable "black".
 *
 *    @PARAM  calibrate   1 if recalibrate the threshold and store unmodified picture
 *                        0 if no calibration and utilize existing threshold
 */
void takePicture( int calibrate )
{
    camera_done = 0;
    while(!camera_done)
    {
        VSYNC = mPORTBReadBits( BIT_15 );

        if (VSYNC == 0x8000)         // if VSYNC goes high, a image is detected, then:
        {
            row = 0;
            black = 0;

            for(i = 0; i< 255 ; i++){
                hst[i] = 0;
            }
            while(row < Image_row){         // if the image not reach enough col
                HREF = mPORTAReadBits( BIT_2 );
                if( HREF == 0x0004 & oHREF == 0x0000){
                   // if a rising edge is detected
                    col = 0;
                    Y = 0;
                    while (col < Image_col){
                        PCLK = mPORTAReadBits( BIT_4 );
                        if (PCLK != 0x0000 & oPCLK == 0x0000){
                            if (Y == 1){                    // get Y leave
                                //image_test[col][row] =  (mPORTBReadBits( BIT_10 | BIT_11 | BIT_12 )>> 5);
                                if(calibrate)
                                {
                                    
                                    image_test[col][row] = (mPORTBReadBits( BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7 )>>3) | (mPORTBReadBits( BIT_10 | BIT_11 | BIT_12 )>> 5);
                                    hst[image_test[col][row]]++;
                                }
                                else
                                {
                                    temp = (mPORTBReadBits( BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7 )>>3) | (mPORTBReadBits( BIT_10 | BIT_11 | BIT_12 )>> 5);
                                    //hst[temp]++;
                                    if(temp > thresh)
                                    {
                                        image_test[col][row] = 255;                                    
                                    }
                                    else
                                    {
                                        image_test[col][row] = 0;
                                        black++;
                                    }
                                }
                                col++;
                                Y = 0;  
                                
                            }
                            else Y = 1;
                        }
                        oPCLK = PCLK;
                   }

                   row++;
                }
                oHREF = HREF;
            }
            camera_done = 1;
            
            //calibration step
            if(calibrate)
            {
                sum = 0;
                for (i = 0; i < 256; i++)
                {
                    sum += i * hst[i];
                }
                sumB = 0;
                wB = 0;
                wF = 0;
                max = 0;
                between = 0;
                threshold1 = 0;
                threshold2 = 0;
                
                for (i = 0; i < 256; i++)
                {
                    wB += hst[i];
                    if (wB != 0)
                    {
                        wF = total - wB;
                        if (wF == 0)
                            break;
                        sumB += i * hst[i];
                        mB = sumB / wB;
                        mF = (sum - sumB) / wF;
                        between = wB * wF * (mB - mF) * (mB - mF);
                        if ( between >= max )
                        {
                            threshold1 = i;
                            if ( between > max ) {
                                threshold2 = i;
                            }
                            max = between;
                        }
                    }
                }
                thresh = (threshold1 + threshold2) / 2;
            }


//            if(calibrate)
//            { 
//                max1 = 0;
//                max2 = 0;
//                max3 = 0;
//                max_value = 0;
//                temp = 0;
//                for (i = 0; i < 255; i++)
//                {
//                    if(max_value < hst[i])
//                    {
//                        max_value = hst[i];
//                        max1 = i;
//                    }
//                }
//                max_value = 0;
//                for ( i = 0; i < (max1-50); i++)
//                {
//                    if(max_value < hst[i])
//                    {
//                        max_value = hst[i];
//                        max2 = i;
//                    }
//                }
//                max_value = 0;
//                for (i = (max1+50); i < 255; i++)
//                {
//                    if(max_value < hst[i])
//                    {
//                        max_value = hst[i];
//                        max3 = i;
//                    } 
//                }
//                if ( hst[max3] > hst[max2] ) thresh = (max3 + max1)/2;
//                else if (hst[max3] <= hst[max2] ) thresh = (max2 + max1)/2;
//                //thresh = 50;
//                tft_fillRect(10,300,200,15,ILI9340_BLACK); 
//                tft_setCursor(10,200);
//                sprintf(buffer," %d %d %d %d\n %d",thresh,max1, max2, max3, hst[50]);
////                tft_writeString(buffer);
//            }
        
            
              

            
//            tft_fillRect(10,300,200,10,ILI9340_BLACK); 
//            tft_setCursor(10,300);
//            sprintf(buffer," %d %d %d ", max1, max2, max3);
//            tft_writeString(buffer);
            //PT_WAIT_UNTIL(pt,display_done);
        //}
        }
    }
}
    int i,j;
    int color;

/*
 * picture_display()
 *    Displays the full picture onto the TFT display. Utilizes global variable "image_test"
 *    as the input for data.
 */    
void picture_display()
{
    tft_fillRect(3,3, 234, 300, ILI9340_BLACK);// x,y,w,h,color
    for (i = 0; i< Image_row ; i++){
        for ( j = 0 ; j < Image_col ; j++){
            Y = image_test[j][i];
            color = (Y>>3) | ((Y>>2)<<5) | ((Y>>3)<<11);
            tft_drawPixel( i+40, j+50, color ); 
        }
    }
}