#include "mbed.h"
#include "rtos.h"
#include "Servo.h"
#include "uLCD_4DGL.h"

DigitalOut myled(LED1);
Servo myservobody(p21);
Servo myservopen(p22);
Servo myservohead(p23);
float min_move = 0.02;
int max_pixel = 50;
float min_wait = 0.02;
float pen_wait = 0.08;
float pen_down = 1;
float pen_up = 0.7;
uLCD_4DGL uLCD(p9,p10,p11);
int tlrow = 0;
int tlcol = 0;
int trow = 0;
int tcol = 0;
int temp1;
int temp2;
Thread thread;
char printData[50][50]; //initialize 2d array here

int temp = 0;

void lcd()
{

    uLCD.cls();
    uLCD.printf("row:%d\ncol:%d",tlrow,tlcol);
    uLCD.printf("\nsv1:%d\nsv2:%d",(int)(myservobody*100),(int)(myservohead*100));
}

int main()
{
    uLCD.baudrate(3000000);
    myservopen = pen_up;
    myservohead = 0;
    myservobody = 0;

    for(float i=0; i<=1.0; i+= min_move) {
        for(float j=0; j<=1.0; j+= min_move) {
            tlrow = (int)(i/min_move);
            tlcol = (int)(abs(j - 1.0)/min_move);
            temp = printData[tlrow][tlcol];
            if(temp == 1) {
                thread.start(lcd);
                if(myservobody>i) {
                    while(myservobody>i) {
                        myservobody = myservobody - min_move;
                        wait(min_wait);
                    }
                } else {
                    while(myservobody<i) {
                        myservobody = myservobody + min_move;
                        wait(min_wait);
                    }
                }

                if(myservohead>j) {
                    while(myservohead>j) {
                        myservohead = myservohead - min_move;
                        wait(min_wait);
                    }
                } else {
                    while(myservohead<j) {
                        myservohead = myservohead + min_move;
                        wait(min_wait);
                    }
                }
                myservohead = j;
                myservobody = i;
                myled = 1;
                myservopen = pen_down;
                wait(pen_wait);


                myservopen = pen_up;
                wait(pen_wait);
                myled = 0;
                tcol = tlcol;
                trow = tlrow;
            }
        }
    }
}