File content as of revision 0:d69efd0ee139:

/*****************************************************************************************************
DUMB BOX firmware 1.0
07/30/14

Takes picture at time 0.  Takes picture at set time.  Send 2 jpeg files upon request from Android phone.
A switch takes the first picture.  
Bluetooth input data to set time to take second picture.
Send picture via Bluetooth, if possible.

***************************************************************************************************/


//
//  Pin function
//  P5  
//  P6  
//  P7  
//  P8  
//  P9  UART_TX; Bluetooth interface TX (Bluetooth side: RX)
//  P10 UART_RX; Bluetooth interface RX (Bluetooth side: TX)
//  P11  
//  P12 
//  P13 
//  P14 
//  P15 
//  P16 
//  P17 
//  P18 
//  P19 
//  P20 Battery Analog In
//  P21 Green LED 1
//  P22 RED LED 2
//  P23 LED illumination    //LW514 LED WHITE ROUND CLEAR 5mm; 897-1183-ND
//  P24 
//  P25 Bluetooth On/Off
//  P26
//  P27 UART_RX; Image Sensor interface (Camera side: TX)
//  P28 UART_TX; Image Sensor interface (Camera side: RX)
//  P29 Start_Button
//  P30 
//
//

#include "mbed.h"
#include "Camera_LS_Y201.h"
#include "SDFileSystem.h"
#include "SimpleJpegDecode.h"
#include "pb_decode.h"
#include "pb_encode.h"
#include "pb.h"
#include "vocit4.pb.h"
#include "iSerial.h"


#define DEMO_FILE  "/local/input.jpg"
#define DEBMSG      printf
#define NEWLINE()   printf("\r\n")
#define image_width 480
#define image_height 640
#define USE_SDCARD 0
#define Setting "/local/setting.txt"
#define ON  true
#define OFF false
#define Pressed 0


/*
#if USE_SDCARD
#define FILENAME    "/sd/IMG_%04d.jpg"
SDFileSystem fs(p5, p6, p7, p8, "sd");
#else
#define FILENAME    "/local/IMG_%04d.jpg"
LocalFileSystem fs("local");
#endif
Camera_LS_Y201 cam1(p28, p27);//(p13, p14);
*/

#if USE_SDCARD
    #define FILENAME    "/sd/INIT%03d.jpg"
    #define FILENAME2   "/sd/FINAL%03d.jpg"
    SDFileSystem fs(p5, p6, p7, p8, "sd");
#else
    #define FILENAME    "/local/INIT%03d.jpg"
    #define FILENAME2   "/local/FINAL%03d.jpg"
    LocalFileSystem fs("local");
#endif
Camera_LS_Y201 cam1(p28, p27);//(p13, p14);
SimpleJpegDecode decode;

//_____________________________________________________________________________________________________
//Camera set up

typedef struct work {
    FILE *fp;
} work_t;

work_t work;

/**
 * Callback function for readJpegFileContent.
 *
 * @param buf A pointer to a buffer.
 * @param siz A size of the buffer.
 */
void callback_func(int done, int total, uint8_t *buf, size_t siz) {
    fwrite(buf, siz, 1, work.fp);

    static int n = 0;
    int tmp = done * 100 / total;
    if (n != tmp) {
        n = tmp;
        if ((n % 10) == 0)
        {
            DEBMSG("Writing...: %3d%%", n);
            NEWLINE();
        }
    }
}

/**
 * Capture.
 *
 * @param cam A pointer to a camera object.
 * @param filename The file name.
 *
 * @return Return 0 if it succeed.
 */
int capture(Camera_LS_Y201 *cam, char *filename) {
    /*
     * Take a picture.
     */
    if (cam->takePicture() != 0) {
        return -1;
    }
    DEBMSG("Captured.");
    NEWLINE();

    /*
     * Open file.
     */
    work.fp = fopen(filename, "wb");
    if (work.fp == NULL) {
        return -2;
    }

    /*
     * Read the content.
     */
    DEBMSG("%s", filename);
    NEWLINE();
    if (cam->readJpegFileContent(callback_func) != 0) {
        fclose(work.fp);
        return -3;
    }
    fclose(work.fp);

    /*
     * Stop taking pictures.
     */
    cam->stopTakingPictures();

    return 0;
}
 
//_________________________________________________________________________________________________________
//General I/O and COM set up


Serial pc(USBTX, USBRX); // tx, rx, using usb program port for debug
iSerial bluetooth(p9, p10);  // UART interface to bluetooth module

//Input
DigitalIn Start(p29);
AnalogIn Batt_stat(p20);

//Output
DigitalOut led_green(p21);
DigitalOut led_red(p22);
DigitalOut led_illuminate(p23);

DigitalOut Camera_power(p24);
//DigitalOut Bluetooth_power(p25);

//Variables

uint8_t buffer[9000];
int bufferSize = 0;
char command;

bool bluetooth_state;
bool bluetooth_connected;
bool NACK_received, ACK_received;
bool start_sending_pic;
bool Request_image;
bool timer_started;
Timer wait_timer, wait_ack_timer;

int Wait_Time_pic;
int cnt;



//_____________________________________________________________________________________________
//Bluetooth Com functions

bool wait_ack = false;


void send_ack(bool x)
{
    BaseMessage BaseMsg;
    //uint8_t outbuffer[1024];
    
    
    pb_ostream_t outStream = pb_ostream_from_buffer(buffer, sizeof(buffer));
    
    BaseMsg.ack.ok = x;
    BaseMsg.has_ack = true;
    BaseMsg.has_preferences = false;
    BaseMsg.has_images = false;
    BaseMsg.has_time = false;
    BaseMsg.has_request = false;
    BaseMsg.has_packet = false;
                            
    int status = pb_encode(&outStream, BaseMessage_fields, &BaseMsg);
    uint8_t message_length = outStream.bytes_written;
    pc.printf("sending ack %d.  Size:%d\r\n", x, message_length);
    pc.printf("\r\n");
        for(int i = 0; i < message_length; i++)
        {                     
            pc.printf("%u,",buffer[i]);
        } 
        pc.printf("\r\n");
    if(status)
    {      
        for(int i = 0; i < message_length; i++)
        {                  
            bluetooth.putc(buffer[i]);
            wait_ms(1);
        }
    }
    else
    {
        pc.printf("failed to send ack\r\n");
    }
}


bool decode_message()
{
    
    pb_istream_t inStream;
    BaseMessage baseMsg;
    bool result = false;
    
    while(bluetooth.readable()) 
    {
        buffer[bufferSize] = bluetooth.getc();
        bufferSize++;
        wait_us(80);
    }
    int i = 0;
    while(i < bufferSize)
    {
        pc.putc(buffer[i]);
        i++;
    }
    pc.printf("\r\n");
    i = 0;
    while(i< bufferSize)
    {
        pc.printf("%d,", buffer[i]);
        i++;
    }
    
    pc.printf("\r\nbyte size receive %d\r\n", bufferSize);
    
    NEWLINE();
    if (bufferSize > 0)
    {
        inStream = pb_istream_from_buffer(buffer, bufferSize);
        int status = pb_decode(&inStream, BaseMessage_fields, &baseMsg);
            
        if (status)
        {
            bufferSize = 0;
            if((baseMsg.messageType == BaseMessage_MessageType_Ack) && baseMsg.has_ack)//receive Ack
            {
                pc.printf("ACK message type.\r\n");
                
                Ack ackMessage = baseMsg.ack;
                pc.printf("has_ack is %d and ack value is %d and %d\r\n", baseMsg.has_ack, baseMsg.ack.ok, ackMessage.ok);
                if (ackMessage.ok)
                {
                    wait_ack = false;
                    wait_ack_timer.stop();
                    wait_ack_timer.reset();
                    result = true;  //able to decode ack and remove queue 
                    ACK_received = true;
                    
                }
                else 
                {
                    wait_ack = false;
                    NACK_received = true;
                    result = true;     //NACK received but effectively decoded message
                }
            }
        
            else if(baseMsg.messageType == BaseMessage_MessageType_Preferences)//setting change
            {
                pc.printf("Preference message type.\r\n");
                if(baseMsg.has_preferences)
                {
                    Wait_Time_pic = baseMsg.preferences.timeInterval;
                    FILE* fp;
                    fp = fopen(Setting, "w");
                    fprintf(fp, "%d\r\n", cnt);
                    fprintf(fp, "%d", Wait_Time_pic);
                    fclose(fp);
                    pc.printf("wait time changed to %d", Wait_Time_pic);
                
                }
                result = true;
                send_ack(true);
            }
           else if(baseMsg.messageType == BaseMessage_MessageType_RequestImages)
            {
                pc.printf("Request Images message type.\r\n");
                pc.printf("has_request is %d\r\n", baseMsg.has_request);
                pc.printf("baseMsg.request.request is %d\r\n", baseMsg.request.request);
                if(baseMsg.has_request)
                {
                    if(baseMsg.request.request)
                    {
                        send_ack(true);
                        Request_image = true;
                    }
                }
                result = true;
            }
        }
        else
        {
            char* stringcomp = strstr((char *)buffer, "RING");
            char* stringcomp2 = strstr((char *)buffer, "RFCOMM");
            char* stringcomp3 = strstr((char *)buffer, "NO CARRIER");
            char* stringcomp4 = strstr((char *)buffer, "SET BT NAME");
            char* stringcomp5 = strstr((char *)buffer, "INQUIRY");
            
            if(stringcomp != NULL && stringcomp2 !=NULL)
            {
                bluetooth_connected = true;
                pc.printf("Bluetooth has connection\r\n");
                
            }
            else if(stringcomp3 != NULL)
            {
                bluetooth_connected = false;
                pc.printf("Bluetooth not connected\r\n");
            }
            else if ((stringcomp4 != NULL) && (stringcomp5 != NULL))
            {
                
            }
            else
            {
                pc.printf("Failed to decode message.\r\n");
                send_ack(false);
            }
            
            bufferSize = 0;
            result = false;
        }
    }
    
    return result;  //false only when NACK or not able to decode
}

int GetFileSize(char fname[64])
{
    int size;
    
    FILE* pFile = fopen (fname,"rb");
    if (pFile==NULL)
    { 
        size = -1;
        pc.printf("Error opening file");
    }
    else
    {
        fseek (pFile, 0, SEEK_END);   // non-portable
        size=ftell (pFile);
        fclose (pFile);
        printf ("Size of myfile.txt: %ld bytes.\n",size);
    }
    return size;   
}

bool first_pic_started, second_pic_started;
bool first_pic_completed, second_pic_completed;
bool init_first_pic, init_second_pic;
int num_of_packets;

//send the number of packet to expect
//then send the first packet after ACK (about 7 packet)
//false means fail to encode
bool send_pic(int index)    
{  
    BaseMessage BaseMsg;
    char fname[64];
    int size;
    
    //FILE *fp;
    
    if (cnt > 0)    //# of pic taken
    {
        if(index == 1)//first file
        {
            //get filesize
            snprintf(fname, sizeof(fname) - 1, FILENAME, cnt - 1);
            init_first_pic = true;
        }
        else
        {
            snprintf(fname, sizeof(fname) - 1, FILENAME2, cnt - 1); 
            init_second_pic = true;
        }
    
        size = GetFileSize(fname); 
        
        if(size == -1)
        {
            pc.printf("Error getting size (%d) of File.  File cnt: %d \r\n", size, cnt);
            //ERROR
            init_first_pic = false;
            init_second_pic = false;
            return false;
        }
        else
        {
            pb_ostream_t outStream = pb_ostream_from_buffer(buffer, sizeof(buffer));
            BaseMsg.messageType = BaseMessage_MessageType_Images;
            BaseMsg.has_images = true;
            BaseMsg.has_time = false;
            BaseMsg.has_packet = false;
            BaseMsg.has_preferences = false;
            BaseMsg.has_ack = false;
            BaseMsg.has_request = false;
            if(index == 1)
            {
                BaseMsg.images.isFirstImage = true;
            }
            else
            {
                BaseMsg.images.isFirstImage = false;
            }
            BaseMsg.images.fileSize = size;
            num_of_packets = ceil((float)(size/8192));
            BaseMsg.images.numberOfPackets = num_of_packets;
            int status = pb_encode(&outStream, BaseMessage_fields, &BaseMsg);
            uint8_t message_length = outStream.bytes_written;
            pc.printf("Sending image info.  File size: %d, num of packet: %d, isFirstimage: %d\r\n", size, num_of_packets, BaseMsg.images.isFirstImage);
            if (status)
            { 
                for(int i = 0; i < message_length; i++)
                {                     
                    bluetooth.putc(buffer[i]);
                    wait_ms(1);
                } 
                wait_ack = true;
                NACK_received = false;
                ACK_received = false;
                start_sending_pic = true;
                wait_ack_timer.reset();
                wait_ack_timer.start();
                return true;
            }
            else    //failed to encode
            {   
                pc.printf("failed to encode message to send out of bluetooth\r\n");
                
                return false;
            }
        }
    }
    else
    {
        //init_first_pic = false;
        //init_second_pic = false;
        pb_ostream_t outStream = pb_ostream_from_buffer(buffer, sizeof(buffer));
        BaseMsg.messageType = BaseMessage_MessageType_Time;
        BaseMsg.has_images = false;
        BaseMsg.has_time = true;
        if(timer_started)
        {
            BaseMsg.time.remainingTime = Wait_Time_pic - wait_timer.read();
        }
        else
        {
            BaseMsg.time.remainingTime = -1;
        }
        BaseMsg.has_ack = false;
        BaseMsg.has_preferences = false;
        BaseMsg.has_packet = false;
        BaseMsg.has_request = false;
                            
        int status = pb_encode(&outStream, BaseMessage_fields, &BaseMsg);
        uint8_t message_length = outStream.bytes_written;
        pc.printf("sending remaining time: %d. hex: %x.  Size:%d\r\n", BaseMsg.time.remainingTime, BaseMsg.time.remainingTime, message_length);
        pc.printf("\r\n");
        for(int i = 0; i < message_length; i++)
        {                     
            pc.printf("%u,",buffer[i]);
        } 
        pc.printf("\r\n");
        if (status)
        { 
            for(int i = 0; i < message_length; i++)
            {                     
                bluetooth.putc(buffer[i]);
                wait_ms(1);
            } 
            wait_ack = true;
            NACK_received = false;
            ACK_received = false;
            wait_ack_timer.reset();
            wait_ack_timer.start();
            return true;
        }
        else    //failed to encode
        {
            pc.printf("failed to encode message to send out of bluetooth\r\n");
           
            return false;
        }
        
    }
    
}

bool send_packet(int file_index, int packet_index)
{
    char fname[64];
    BaseMessage BaseMsg;
    int packet_buf[8192];
    size_t packet_size;
    
    if(file_index == 1)//first file
    {
        //get filesize
        snprintf(fname, sizeof(fname) - 1, FILENAME, cnt - 1);
            
    }
    else
    {
        snprintf(fname, sizeof(fname) - 1, FILENAME2, cnt - 1);     
    }
    
    pb_ostream_t outStream = pb_ostream_from_buffer(buffer, sizeof(buffer));
    BaseMsg.messageType = BaseMessage_MessageType_ImagePacket;
    BaseMsg.has_images = false;
    BaseMsg.has_time = false;
    BaseMsg.has_ack = false;
    BaseMsg.has_preferences = false;
    BaseMsg.has_packet = true;
    BaseMsg.has_request = false;
    //seek file location to send next 8K packet
    FILE* fp = fopen (fname,"rb");
    packet_size = 0;
    if(fp != NULL)
    {
        //seek file location to read
        fseek(fp, (packet_index * 8192),SEEK_SET);
        
        //read and fill buffer, increment packet size; 
        do
        {
            packet_buf[packet_size] = fgetc(fp);
            packet_size++;    
        } while((packet_size < 8192) && (packet_buf[packet_size - 1] != EOF));
    }
    BaseMsg.packet.packetNumber = packet_index;
    BaseMsg.packet.image.size = packet_size;
    for(int i = 0; i < packet_size; i++)
    {
        BaseMsg.packet.image.bytes[i] = packet_buf[i];               
    }
    int status = pb_encode(&outStream, BaseMessage_fields, &BaseMsg);
    uint8_t message_length = outStream.bytes_written;
    pc.printf("sending packet #%d.  Size:%d\r\n", packet_index, message_length);
    if(status)
    {
        for(int i = 0; i < message_length; i++)
        {                     
            bluetooth.putc(buffer[i]);
            wait_ms(1);
        } 
        wait_ack = true;
        NACK_received = false;
        ACK_received = false;
        wait_ack_timer.reset();
        wait_ack_timer.start();
        return true;
    }
    else    //failed to encode
    {
        pc.printf("failed to encode message to send out of bluetooth\r\n");
        
        return false;
    }

}

bool Camera_init(bool camera_pwr)
{
    bool result;
    
    if(camera_pwr == ON)
    {
        Camera_power = 1;
        led_illuminate = 1;
    
        DEBMSG("Camera module");
        NEWLINE();
        DEBMSG("Resetting...");
        NEWLINE();
    
        result = false;
    
        wait(3);

        if (cam1.reset() == 0) 
        {
            DEBMSG("Reset OK.");
            NEWLINE();
            if(cam1.setImageSize(cam1.ImageSize640x480) == 0)
            {
                DEBMSG("Image Size OK.");
                NEWLINE();
                result = true;
            }
            else
            {
                DEBMSG("Image Size Error.");
                NEWLINE();
            }
            
        } 
        else 
        {
            DEBMSG("Reset fail.");
            NEWLINE();
            //error("Reset fail.");
        }
    }
    else
    {
        Camera_power = 0;
        led_illuminate = 0;
        result = true;    
    }
    
    return result;
}

void bluetooth_init()
{
    
    bluetooth.baud(115200);
    //bluetooth.printf("AT\r\n");
    bluetooth.printf("SET BT NAME COLOR_READER_02\r\n");
    bluetooth.printf("INQUIRY 5\r\n");  
                
}               
   
//_______________________________________________________________________________________
//power up load settings


void load_setting(void)
{
    FILE* fp;
 
    pc.printf("Loading Setting.....\r\n\r\n");
    
    fp = fopen(Setting, "r");
    if (fp != NULL)
    {
        fscanf(fp, "%d%d", &cnt, &Wait_Time_pic);
        //fscanf(fp, "%d", &Wait_Time_pic);
        fclose(fp);
    }
    else
    {
        cnt = 0;
        Wait_Time_pic = 10; //10 sec. for debug
    }
    
    pc.printf("Wait time for pic is %d.\r\n", Wait_Time_pic);
    pc.printf("File index is %d.\r\n\r\n", cnt);
    
}



void delete_file(char fname[64])
{
    //snprintf(fname, sizeof(fname) - 1, FILENAME, index-10);           
    remove(fname);                
}


//Take pic and store in memory
//Need to get naming convention to link the image together(1st and 2nd)       
void take_pic(char fname[64])
{
    bool camera_work = Camera_init(ON);
    if(camera_work)
    {
        wait(8);
        int r = capture(&cam1, fname);
        if (r == 0) {
            DEBMSG("[%04d]:OK.", cnt);
            NEWLINE();
            //command = 'd';
        } 
        else 
        {
            DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
            NEWLINE();
            //error("Failure.");
            //command = ' ';
            //mode = 98;  
            //change_mode_screen(mode); 
        }
        
        camera_work = Camera_init(OFF);
                
    }          
}

void Init(void)
{
    timer_started = false;
    Request_image = false;
    init_first_pic = false;
    init_second_pic = false;
    first_pic_started = false;
    second_pic_started = false;
    first_pic_completed = false;
    second_pic_completed = false;
    start_sending_pic = false;
    
    NACK_received = false;
    ACK_received = false;
    wait_ack = false;
    Start.mode(PullUp);
    Camera_power = 0;
    wait_timer.stop();
    wait_timer.reset();
    wait_ack_timer.stop();
    wait_ack_timer.reset();
    load_setting();
    bluetooth_init();
}



int main() { 
    char fname[64];
    
    bool result = false;
    //bool error_cap = false;
    bool test_ended = true;
    bool final_img = false;
    bool temp_result;
    int attempt = 0;
    FILE *fp;
    int packet_index = 0;
    
    
    pc.printf("******************Program Start Here (V2.0).....\n\n\r");
    Init();
    pc.printf("Initialized: Entering while loop.\r\n\r\n");
    while (1) 
    {
        //bluetooth income data --> feeds time to take second picture
        if(bluetooth.readable())
        {
            pc.printf("Message receiving.\r\n");
            result = decode_message();
        }
        
        if(result)  //successful decode Bluetooth packet
        {
            result = false;
            
            if(Request_image)
            {
                Request_image = false;
                pc.printf("Initiate to send first pic.\r\n");
                send_pic(1);
            }
            
            //NACK received
            if(NACK_received)
            {
                wait_ack = false;
                NACK_received = false;
                wait_ack_timer.reset();
                attempt = 0;
                pc.printf("NACK received\r\n\r\n");
                //send_pic(1); resend last packet...not pic
                if(init_first_pic)  //initiate again    
                {
                    pc.printf("Re-Initiate sending Pic 1.\r\n");
                    send_pic(1);
                }
                else if(first_pic_started)  //resend last packet of 1st pic
                {
                    pc.printf("Resending packet number #%d of first pic.\r\n", packet_index - 1);
                    packet_index--;
                    if(packet_index < num_of_packets)
                    {
                        temp_result = send_packet(1, packet_index);
                        if(temp_result)
                        {
                            packet_index++;
                        }
                        else
                        {
                            pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                        }
                    }
                    else
                    {
                        first_pic_completed = true;
                        first_pic_started = false;
                    } 
                }
                
                else if(init_second_pic)    //re-initiate 2nd pic
                {
                    pc.printf("Re-Initiate sending Pic 2.\r\n");
                    send_pic(2);
                }
                else if(second_pic_started) //resend last packet of 2nd pic
                {
                    pc.printf("Resending packet number #%d of second pic.\r\n", packet_index - 1);
                    packet_index--;
                    if(packet_index < num_of_packets)
                    {
                        temp_result = send_packet(2, packet_index);
                        if(temp_result)
                        {
                            packet_index++;
                        }
                        else
                        {
                            pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                        }
                    }
                    else
                    {
                        second_pic_completed = true;
                        second_pic_started = false;
                    } 
                }
                
            }
        
            if(ACK_received)    //Send result via bluetooth, up on request
            {
                pc.printf("ACK received\r\n\r\n");
                ACK_received = false;
                wait_ack = false;
                wait_ack_timer.stop();
                wait_ack_timer.reset();
                attempt = 0;
                if(init_first_pic)
                {
                    pc.printf("Initiating to send first pic\r\n");
                    init_first_pic = false;
                    packet_index = 0;
                    first_pic_started = true;
                    temp_result = send_packet(1, packet_index);    //send first
                    if(temp_result)
                    {
                        packet_index++;
                    }
                    else
                    {
                        pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                    }
                }
                 
                else if(first_pic_started)
                {
                    pc.printf("Send packet #%d of first pic.\r\n", packet_index);
                    if(packet_index < num_of_packets)
                    {
                        temp_result = send_packet(1, packet_index);    
                        if(temp_result)
                        {
                            packet_index++;
                        }
                        else
                        {
                            pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                        }
                    }
                    else
                    {
                        first_pic_completed = true;
                        first_pic_started = false;
                    }
                }    
                
                if(first_pic_completed && !init_second_pic)//init second_pic
                {
                    pc.printf("Initiating to send 2nd pic\r\n");
                    first_pic_completed = false;
                    send_pic(2);
                }
                
                else if(init_second_pic)
                {
                    pc.printf("Sending packet #%d of pic 2\r\n", packet_index);
                    init_second_pic = false;
                    packet_index = 0;
                    second_pic_started = true;
                    temp_result = send_packet(2, packet_index);    //send second
                    if(temp_result)
                    {
                        packet_index++;
                    }
                    else
                    {
                        pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                    }
                }
                 
                else if(second_pic_started)
                {
                    if(packet_index < num_of_packets)
                    {
                        temp_result = send_packet(2, packet_index);    //send second
                        if(temp_result)
                        {
                            packet_index++;
                        }
                        else
                        {
                            pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                        }
                    }
                    else
                    {
                        second_pic_completed = true;
                        second_pic_started = false;
                    }
                }    
                
                if(second_pic_completed)
                {
                    pc.printf("Deleting files\r\n");
                    second_pic_completed = false;
                    
                    snprintf(fname, sizeof(fname) - 1, FILENAME, cnt - 1);
                    delete_file(fname);
                    snprintf(fname, sizeof(fname) - 1, FILENAME2, cnt - 1);
                    delete_file(fname);
                    cnt--;
                    fp = fopen(Setting, "w");
                    fprintf(fp, "%d\r\n", cnt);
                    fprintf(fp, "%d", Wait_Time_pic);
                    fclose(fp);
                    
                }
                
            }
        
            
        }
       
        if ((Start == Pressed) && !timer_started && test_ended)    //Button pressed, take first pic
        {
           pc.printf("Start Test................\r\n");
           wait_ms(200);
           if(Start == Pressed)
           {
            snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
            take_pic(fname); 
            wait_timer.reset();
            wait_timer.start();
            timer_started = true;
            test_ended = false;
           }
        }
       /* else if (Start == 1)
        {
            timer_started = false;
            final_img = false;
            wait_timer.stop();
            test_ended = true;
        }*/
        
        if((wait_timer.read() > Wait_Time_pic) && !final_img)    //After set time, take second pic
        {
            pc.printf("Take 2nd pic\r\n");
            snprintf(fname, sizeof(fname) - 1, FILENAME2, cnt); 
            take_pic(fname); 
            wait_timer.stop();  
            wait_timer.reset();
            timer_started = false;
            cnt++;
            fp = fopen(Setting, "w");
            fprintf(fp, "%d\r\n", cnt);
            fprintf(fp, "%d", Wait_Time_pic);
            fclose(fp);
            final_img = true;
            pc.printf("file size of 2nd pic is %d bytes.\r\n", GetFileSize(fname));
        }
        
        if((wait_ack_timer.read() > 3) && wait_ack)   //Need to edit to resend last packet
        {
            pc.printf("ACK timer timed out.\r\n");
            wait_ack_timer.reset();
            attempt++;
            if(init_first_pic)
            {
                pc.printf("Re-Initiate sending Pic 1.\r\n");
                send_pic(1);
            }
            else if(first_pic_started)
            {
                pc.printf("Resending packet number #%d of first pic.\r\n", packet_index - 1);
                packet_index--;
                if(packet_index < num_of_packets)
                {
                    temp_result = send_packet(1, packet_index);
                    if(temp_result)
                    {
                        packet_index++;
                    }
                    else
                    {
                        pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                    }
                }
                else
                {
                    first_pic_completed = true;
                    first_pic_started = false;
                } 
            }
                
            else if(init_second_pic)
            {
                pc.printf("Re-Initiate sending Pic 2.\r\n");
                send_pic(2);
            }
            else if(second_pic_started)
            {
                pc.printf("Resending packet number #%d of second pic.\r\n", packet_index - 1);
                packet_index--;
                if(packet_index < num_of_packets)
                {
                    temp_result = send_packet(2, packet_index);
                    if(temp_result)
                    {
                        packet_index++;
                    }
                    else
                    {
                        pc.printf("FAIL to decode packet: %d\r\n", packet_index - 1);
                    }
                }
                else
                {
                    first_pic_completed = true;
                    first_pic_started = false;
                } 
            }
        }
        
        if(attempt > 2)
        {
            pc.printf("Three attempts already...no more sending");
            init_first_pic = false;
            init_second_pic = false;
            first_pic_started = false;
            second_pic_started = false;
            first_pic_completed = false;
            second_pic_completed = false;
            start_sending_pic = false;
            attempt = 0;
            wait_ack = false;
            wait_ack_timer.stop();
            wait_ack_timer.reset();
            timer_started = false;
        }
            
        
    }
    
}