#include "sowing.h"
#include "ui_sowing.h"
#include <QThread>
#include "mainwindow.h"
#include <iostream>

#include <QMessageBox>

#include <QFile>
#include <QCoreApplication>
#include <QTextStream>
#include <QDir>

//#include<windows.h>

using namespace std;

int fieldSize1 = 40;    //default, wordt erna nog aangepast aan prentbreedte
QImage prent1;

bool joy=false;

int bl =0;

QString progStr ="";

QString mainProg = "double pi=3.1415; // \n Servo klservo;  // object voor kleine servo// \n Servo grservo;  // object voor grote servo// \n Servo miniservo; // object voor mini servo// \n // \n int pin_gezaaid=4;     // bepaling van welke outputs op welke poorten staan// \n int pin_rood_groot=5;// \n int pin_groen_groot=6;// \n int pin_rood_klein=7;// \n int pin_groen_klein=8;// \n // \n int pin_joystick_x=A0; // bepaling van welke inputs op welke poorten staan// \n int pin_joystick_y=A1;// \n int pin_joystick_druk=A2;// \n int pin_pot_groot=A3;// \n int pin_pot_klein=A4;// \n int pin_IRsense=A5;// \n // \n int pot_groot=0;  // waarde van de potentiometer van grote servo, wordt gebruikt voor bepaling van de huidige positie van die servo// \n int pot_klein=0;  // idem voor kleine servo// \n int IRvalue=0;    // waarde van de IR sensor// \n // \n double alfa=0;   //deze hoeken en afstanden staan aangeduid op de tekeninging op de site// \n double gamma=0;// \n double phi=0;// \n double theta=0;// \n double beta=0;// \n double R1 = 190;  // \n double R2= 185;// \n int i=0;// \n // \n double quotient_groot=0;  // deze parameters worden verduidelijkt in de procedures waar ze gebruikt worden// \n double quotient_klein=0;// \n double hoek_klein=0;// \n double hoek_groot=0;// \n boolean gezaaid=false;// \n int pogingen=0;// \n long start=0;// \n // \n double stap=5;// \n double xpos=0;// \n double ypos=0;// \n double x_gestuurd=50;// \n double y_gestuurd=50;// \n double x_nieuw;// \n double straal=40;// \n double hoek=0;// \n double val_x= 50;  // \n double val_y= 50;// \n int knop=0;// \n // \n // \n double L=0;// \n void setup() // \n { // \n miniservo.attach(9);  // hang mini servo aan pin 9// \n klservo.attach(10);  // hang kleine servo aan pin 10// \n grservo.attach(11); // hang grote servo aan pin 11// \n miniservo.write(610);  //deze drie lijnen brengen de sevo's in een gewenste beginpositie // \n klservo.write(1500);   //// \n grservo.write(1600);   //// \n pinMode(pin_groen_groot, OUTPUT);  // veltel aan controller welke pinnen als output moeten geschakeld worden// \n pinMode(pin_rood_groot, OUTPUT);// \n pinMode(pin_groen_klein, OUTPUT);// \n pinMode(pin_rood_klein, OUTPUT);// \n pinMode(pin_gezaaid,OUTPUT);// \n Serial.begin(9600);// \n } // \n // \n void loop() // \n { // \n while (joystick_on)   // oneindige lus als de robot bestuurd wordt met de joystick // \n { // \n x_gestuurd = analogRead(A0);// \n y_gestuurd = analogRead(A1);// \n x_gestuurd = map(x_gestuurd, 1023, 180, -4, 4);// \n y_gestuurd = map(y_gestuurd, 114, 900, -4, 4);   // \n val_x=val_x+x_gestuurd;// \n val_y=val_y+y_gestuurd;// \n knop=analogRead(pin_joystick_druk);// \n plaats(val_x,val_y);// \n if (knop<500)// \n {// \n zaaien();// \n }// \n }   // \n for (i=0;i<coordSize;i++){// \n val_x=coordinaten_x[i];// \n val_y=coordinaten_y[i];// \n plaats(val_x,val_y);// \n zaaien();// \n }// \n while (true){// \n digitalWrite(pin_rood_groot,HIGH);    // alle leds doen branden voor aanduiding dat alles gezaaid is// \n digitalWrite(pin_groen_groot,HIGH);// \n digitalWrite(pin_rood_klein,HIGH);    // \n digitalWrite(pin_groen_klein,HIGH);// \n }// \n } // \n // \n void plaats(double x, double y)// \n {// \n x=x+55;    // offset op x en y omdat de as van de grote servo niet in de oorsprong van het zaaiveld ligt// \n y=y-3;     //// \n alfa=atan(y/x);                  // deze berekeningen staan uitgelegd op de site// \n L=x/cos(alfa);                   //// \n beta=acos(L/2/R1);               //// \n gamma=acos(R1*cos(beta)/R2);     //// \n phi=beta+gamma;                  //// \n theta=alfa+beta;                 //// \n theta=(theta-0.298833)/1.020741;   // de waarde van theta en phi moeten worden herschaald zodat ze ook overeenkomen met de werkelijke// \n phi=(phi-0.169081)/0.92362;        // hoeken waarop de servos zullen staan, de calibratieparameters werden appart bepaald// \n zetservos(theta,phi);    // stuur deze waarden naar de servo's// \n }// \n // \n void zetservos(double theta, double phi)// \n {// \n hoek_klein=(phi)/pi*(1140-1910)+1910; // zet de hoek phi om naar een waarde in microseconden pulsbreedte die de servo aanstuurd// \n klservo.write(hoek_klein);// \n hoek_groot=(0.52+theta)/pi*(590-2240)+2240; // idem voor theta// \n grservo.write(hoek_groot);// \n wachten(hoek_groot,hoek_klein); // de procedure wachten zal controlleren of de servo's hun gestuurde positie hebben ingenomen// \n }// \n // \n void wachten(double hoek_groot, double hoek_klein)// \n {// \n pot_groot = analogRead(pin_pot_groot);            // lees de waarde van de potentiometer vd grote servo // \n pot_groot = map(pot_groot, 85, 350, 590, 2240);   // herschaal de waarde van de potentiometer naar een overeenkomstige pulsbreedte// \n quotient_groot = (float)hoek_groot/(float)pot_groot; // als dit quotient 1 is, heeft de grote servo zijn gewenste positie bereikt// \n pot_klein = analogRead(pin_pot_klein);            // lees de waarde van de potentiometer vd kleine servo // \n pot_klein = map(pot_klein, 545, 80, 1140, 1910);  // herschaal de waarde van de potentiometer naar een overeenkomstige pulsbreedte// \n quotient_klein = (float)hoek_klein/(float)pot_klein;  // als dit quotient 1 is, heeft de kleine servo zijn gewenste positie bereikt// \n if (abs(quotient_groot-1)>=0.03)      // als de stelwaarde voor de servos is veranderd sinds deze procedure is opgeroepen zal het quotient niet gelij aan 1 zijn en// \n moeten de indicatorleds veranderd worden// \n {// \n digitalWrite(pin_rood_groot,HIGH);  // rode led wil zeggen dat de servo nog onderwerg is naar zijn gewenste positie// \n digitalWrite(pin_groen_groot,LOW);  // groene led wil zeggen dat de servo zijn gewenste positie heeft bereikt// \n }// \n if (abs(quotient_klein-1)>=0.03)// \n {// \n digitalWrite(pin_rood_klein,HIGH);  // rode led wil zeggen dat de servo nog onderwerg is naar zijn gewenste positie// \n digitalWrite(pin_groen_klein,LOW);  // groene led wil zeggen dat de servo zijn gewenste positie heeft bereikt// \n }// \n while ((abs(quotient_groot-1)>0.02) || (abs(quotient_klein-1)>0.03))  // als de uitgelezen waarde van de potentiometer binnen 3% van zijn gewenste waarde is gekomen// \n wordt veronderstelt dat de servo op de gewenste positie is, zolang dit niet het geval is blijft het // \n {                                                                   // programma de potentiometers uitlezen// \n if (abs(quotient_groot-1)<=0.03)// \n {// \n digitalWrite(pin_rood_groot,LOW);    // rode led wil zeggen dat de servo nog onderwerg is naar zijn gewenste positie// \n digitalWrite(pin_groen_groot,HIGH);  // groene led wil zeggen dat de servo zijn gewenste positie heeft bereikt// \n }// \n if (abs(quotient_klein-1)<=0.03)// \n {// \n digitalWrite(pin_rood_klein,LOW);    // rode led wil zeggen dat de servo nog onderwerg is naar zijn gewenste positie// \n digitalWrite(pin_groen_klein,HIGH);  // groene led wil zeggen dat de servo zijn gewenste positie heeft bereikt// \n }// \n pot_groot = analogRead(A3);                              // de waarde van de potentiometers moet steeds opnieuw worden uitgelezen// \n pot_groot = map(pot_groot, 85, 350, 590, 2240);          //// \n quotient_groot = (float)hoek_groot/(float)pot_groot;     //// \n pot_klein = analogRead(A4);                              //// \n pot_klein = map(pot_klein, 545, 80, 1140, 1910);         //// \n quotient_klein = (float)hoek_klein/(float)pot_klein;     //// \n //    Serial.print(\"klein:   \");// \n //    Serial.print(quotient_klein-1,DEC);// \n //    Serial.print(\"groot  \");// \n //    Serial.println(quotient_groot-1,DEC);// \n }// \n digitalWrite(pin_rood_groot,LOW);        // als de while lus doorlopen is zal voor de servo die het laatst op zijn positie is geraakt de led nog niet veranderd zijn, dat gebeurt hier// \n digitalWrite(pin_groen_groot,HIGH);      //// \n digitalWrite(pin_rood_klein,LOW);        //// \n digitalWrite(pin_groen_klein,HIGH);      //// \n delay(200);  // een kleine delay om ervoor te zorgen dat de servos tot stilstand zijn gekomen// \n }// \n // \n void zaaien()// \n {// \n gezaaid=false; // wordt true al de IRsensor aangeeft dat er een zaadje is gevallen// \n pogingen=0;// \n while (pogingen<3 && gezaaid==false) // zolang er niet gezaaid is zal het programma poginen ondernemen om alsnog een zaadje te doen vallen, na 3 pogingen wordt er verder gegaan// \n {// \n miniservo.write(2500); // breng de miniservo naar zijn onderste positie zodat een zaadje kan vallen// \n start=millis();// \n while (gezaaid==false && (millis()-start)<2000) // gedurende 2 seconden zal gewacht worden of het zaadje is gevallen// \n {// \n IRvalue= analogRead(pin_IRsense);// \n if (IRvalue>500)  // De IRsensor geeft 0 (of iets in die buurt) als er IR straling op valt, als de straling wordt onderbroken (en er dus een zaadje is gevallen)// \n wordt de waarde 1013// \n { // \n gezaaid=true;// \n digitalWrite(pin_gezaaid,HIGH);  // Door deze led kort te doen oplichten wordt de gebruiker vertelt dat er een zaadje is gevallen// \n delay(200);                      //// \n digitalWrite(pin_gezaaid,LOW);   //// \n } // \n }// \n if (gezaaid==false)  // Als na 2 seconden er geen gevallen zaadje is gedetecteerd, gaat het programma er van uit dat de zaadjes opgestopt zitten in de zaaikop en zal de verste arm snel heen en weer bewegen om de zaadjes los te schudden// \n {// \n miniservo.write(610);  // breng de miniservo eerst terug naar zijn hoogste positie// \n delay(500);   // wacht tot de miniservo in zijn hoogste positie is geraakt// \n grservo.write(1500);       // de grote servo wordt in een positie gebracht zodat de verste arm goed kan bewegen// \n klservo.write(1300);  // De kleine servo wordt snel na mekaar in posities gestuurd met ongeveer 30° verschil, hierbij wordt geen gebruikt gemaakt van de procedure zetservos omdat hier niet gewacht moet worden tot de servo in de juiste positie is// \n delay(200);           // het is alleen belangrijk dat de servo de beweging maakt en dit snel genoeg doet zodat de zaadjes worden losgeschud// \n klservo.write(1700);  //// \n delay(200);           //// \n klservo.write(1300);  //// \n delay(200);           //// \n klservo.write(1700);  //// \n delay(200);           //// \n plaats(val_x, val_y);     // na het schudden worden de servos terug in hun juiste positie gebracht// \n } // \n pogingen=pogingen+1;    // \n }// \n if (gezaaid==false)   // als na 3 pogingen er nog geen vallend zaadje gedetecteerd is, zal de gebruiker hiervan gewaarschuwd door de led die gedurende 3seconden blijft branden. Dit kan dan geinterpreteerd worden als teken dat de zaadkop leeg is of er een verstopping// \n {                     // is die niet op bovenstaande manier kan worden opgelost// \n digitalWrite(pin_gezaaid,HIGH);// \n delay(3000);// \n digitalWrite(pin_gezaaid,LOW);// \n }// \n miniservo.write(610);// \n }// \n // \n // \n void druk()// \n {// \n Serial.print(\"x  \");// \n Serial.print(val_x,DEC);// \n Serial.print(\"     \"); // \n Serial.print(\"y:  \");// \n Serial.print(val_y,DEC);// \n Serial.print(\"     \"); // \n Serial.print(\"alfa   \");// \n Serial.print(alfa,DEC);// \n Serial.print(\"     \");// \n Serial.print(\"gamma  \");// \n Serial.print(gamma,DEC);// \n Serial.print(\"     \");// \n Serial.print(\"theta  \");// \n Serial.print(theta,DEC);// \n Serial.print(\"     \");// \n Serial.print(\"phi  \");// \n Serial.println(phi,DEC);// \n }// \n";


//Sowing::Sowing(QImage prent, QWidget *parent) : //start, opent venster
//    QDialog(parent),
//    ui(new Ui::Sowing)
//{
//    FirstTime = true;

//    TimeOutbool= false;
//    pauseSowing =false;

//    fieldSize1=prent.width();
//    fieldSize1=40;
//    prent1=prent;

//    bl = numBlack();

//    ui->setupUi(this);

//    ui->pauseButton->setVisible(false);

//    ui->progressBar->setRange(0,bl);
//    ui->progressBar->setValue(0);




//}

//Sowing::~Sowing()   //sluit venster
//{
//    delete ui;
//}

//void Sowing::updateProgress(int p)  //Update de progressbar
//{
//    ui->progressBar->setValue(p);

//    if (p>bl/3)
//    {
//        if (p>bl/2) ui->label->setText("..the seeds of love..");
//        else ui->label->setText("Sowing the seeds of love..");

//    }
//}

//void Sowing::startPressed()     //Als start/resume knop wordt aangeklikt
//{

//    ui->pauseButton->setVisible(true);
//    ui->startButton->setVisible(false);

//    if (FirstTime==true)    //enkel de eerste keer is het 'start' derna 'resume'
//    {
//        if (pauseSowing==false){                            //Om zeker te zijn dat hij geen meerdere keren in de lus gaat
//            //ui->startButton->setText("pause");
//            FirstTime=false;
//            ui->startButton->setText("Resume");
//            sow();          //Begin te zaaien

//        }
//    }
//    else
//    {

//        pauseSowing=false;
//        ui->pauseButton->setVisible(true);
//        ui->startButton->setVisible(false);


//    }



//    //pauseSowing =true;
//    FirstTime=false;

//}

//void Sowing::cancelPressed() //cancel : sluit venster
//{
//    cancel=true;
//}

//void Sowing::openTimeout() //opent 'timeout' venster
//{
//    QMessageBox msgAbout;
//    msgAbout.setWindowTitle("Timeout");
//    msgAbout.setText("Connection failed, please check your connection with the Arduino");
//    msgAbout.exec();
//    TimeOutbool=true;


//    //startPressed();
//    ui->startButton->setText("Start"); //zet alles stop, verander resume door startt om opnieuw te beginnen
//    pauseSowing=false;


//    cout << "timeout was er" << endl;

//    close();
//}



//void Sowing::sendall()
///*

//  Wegens problemen, arm die op hol slaat bij het doorsturen van data moet deze functie gebruikt worden
//  met de schakelaar op de arduino af, dan zullen alle coordinaten ineens gestuurd worden en later
//  kan er gezaaid worden

//  De verzonden string is van de vorm x3y4x5y6x8y9z
//  dit zaait op coordinaten (0,0), (3,4),(5,6),(8,9). Z sluit de reeks af
// */
//{
//    _Serial poort;
//    poort.SetComPort("COM3" , 9600);

//    std::string letterx="x";
//    std::string lettery="y";
//    std::string letterz="z";
//    char xcoord[10], ycoord[10];
//    itoa (x,xcoord,10);
//    itoa (y,ycoord,10);
//    std::string coord;



//    for (int i=0; i<fieldSize1; i++)
//    {
//        for (int j=0; j<fieldSize1; j++)
//        {
//            if (prent1.pixelIndex(i, j)==0)
//            {
//                itoa (i,xcoord,10);     //integers naar chars omzettten
//                itoa (j,ycoord,10);

//                coord=coord+ letterx + xcoord + lettery + ycoord;   //sting die alles bijeen zet
//            }
//        }
//    }
//    coord=coord +letterz;

//    char str[1600];
//    for(int i=0;i<1600;i++) str[i]=coord[i]; //string2char

//    poort.WriteString( str );  //WriteString
//}

//void Sowing::sow()
//{
//    /*
//      Originele zaaifuncttie, veel properder, aangezien deze zal wachten op de arduino om een 'klaar signaal te geven' voor en na
//      elke coordinaat, produceert een timeout als er geen antwoord binnen de timeouttijd komt.
//      Zal hierdoor ook de progress in t oog kunnen houden en updaten

//      */

//    cancel=false;
//    TimeOutbool=false;

//    ui->pauseButton->setVisible(true);
//    ui->startButton->setVisible(false);

//    waitForReady(TimeOutTime);
//    cout << "eerste keer toch al door" << endl;

//    //TimeOutbool==false;

//  if (TimeOutbool==false)
//  {
//    cout << "false, kan niet" << endl;

//    comm_sendCoord(fieldSize1,fieldSize1);      //Send the Image dimentions first

//    ui->label->setText("Sowing..");


//    progress=0;

//    for (int i=0; i<fieldSize1; i++)
//    {
//        if (cancel==true) break;
//        for (int j=0; j<fieldSize1; j++)
//        {
//            if (cancel==true) break;
//            if (prent1.pixelIndex(i, j)==0)
//            {

//                //comm_waitms(2000);

//                waitForReady(TimeOutTime);

////                if (TimeOutbool==true)
////                {
////                    failedTimeout();
////                    TimeOutbool=false;
////                    //waitForReady(TimeOutTime);        //Verder proberen gaan of direct afsluiten?
////                }

//                comm_sendCoord(i,j);

//                progress++;
//                updateProgress(progress);

//            }

//        }

//    }
//  }

//    cout << "Sowing klaar" << endl;

//    ui->label->setText("Finished!");
//  }


/*
  else {
      //startPressed();
      ui->startButton->setText("Start");
      pauseSowing=false;
      cout << "timeout was er" << endl;
      close();
  }
}
*/


//void Sowing::waitForReady(int Timeout)  //wachtfunctie, wacht op klaar signaal van arduino, anders timeout
//{
//    unsigned char x;
//    int i=0;
//    int q=0;
//        do
//        {
//            comm_waitms(10);
//            comm_readSignal(x);
//            i++;
//            cout << "time" << i << endl;
//            if (x=='A') {
//                cout << "x=A" << endl; q=2;
//            }
//            if (cancel==true){
//                 q=2;
//            }
////            if (pauseSowing==true){
////                 q=2;
////            }

//            //pauseEvents();

//            if (i==Timeout){
//                 cout << "timeout" << endl; TimeOutbool= true; openTimeout(); break;
//            }
//            if (TimeOutbool==true){
//                 q=2;
//            }
//        }
//            while(q<1);

//}

//void Sowing::failedTimeout() //timeout ->zet alles op pause (controlleer manueel wat er mis is)
//{
//        startPressed();
//        do {
//            QCoreApplication::processEvents(QEventLoop::AllEvents, 20);
//            sleep(1);
//        } while (pauseSowing==true);

//        //waitForReady(TimeOutTime);  //kan gevaarlijk worden

//}

//int Sowing::numBlack()      //Count all black pixels in drawing (for calculating progress)
//{
//    int counter=0;

//    for (int i=0; i<fieldSize1; i++)
//    {
//        for (int j=0; j<fieldSize1; j++)
//        {
//            if (prent1.pixelIndex(i, j)==0)
//            {
//                counter++;
//            }
//        }
//    }
//    return(counter);
//}

//void Sowing::pausePressed() //pause aangeklikt, zet op pauze, laat 'resume' buttton zien ipv 'pause'
//{
//        pauseSowing=true;
//        ui->pauseButton->setVisible(false);
//        ui->startButton->setVisible(true);

//        pauseEvents();
//}

//void Sowing::pauseEvents()  //Pausefunctie, wacht tot resume is aangeklikt
//{
//    if (pauseSowing==true)
//    {
//        do {
//            sleep(1);
//            QCoreApplication::processEvents(QEventLoop::AllEvents, 20);
//        } while (pauseSowing==true);

//        //waitForReady(TimeOutTime);
//    }
//}


Sowing::Sowing(QImage prent, QWidget *parent) : //start, opent venster
    QDialog(parent),
    ui(new Ui::Sowing)
{

    fieldSize1=prent.width();
    fieldSize1=40;
    prent1=prent;

    //bl = numBlack();

    ui->setupUi(this);

}

Sowing::~Sowing()   //sluit venster
{
    delete ui;
}

void Sowing::startPressed() //nieuwste versie: 'save' knop
{
    writeFile();
}

void Sowing::cancelPressed()
{

}


void Sowing::writeFile()    //Schrijf de arduino sketch file
{
    coordStr();

    progStr= "#include <Servo.h> // \n" +progStr;


    if (joy==true)  //enable/disable besturing met joystick
    {
        progStr.append("\n bool joystick_on = true; \n");
    }
    else progStr.append("\nbool joystick_on = false; \n");

    progStr.append("//END GUI PRODUCED CODE \n\n");

    mainProg=progStr+mainProg;

    if (!QDir("SowingRobot").exists())
    {
        QDir().mkdir("SowingRobot");
    }

    QFile file("./SowingRobot/SowingRobot.pde");
    file.open(QIODevice::WriteOnly | QIODevice::Text);
    QTextStream out(&file);
    out << mainProg;

    // optional, as QFile destructor will already do it:
    file.close();
    close();
}

void Sowing::coordStr() //maakt arrays van de x en y waarden
{
    QString coordX = "double coordinaten_x[";
    QString coordY = "double coordinaten_y[";
    int coordSize = 0;
    bool first = true;

    for (int i=0; i<fieldSize1; i++)
    {
        for (int j=0; j<fieldSize1; j++)
        {
            if (prent1.pixelIndex(i, j)==0)
            {
                if (first==false)
                {
                coordX.append(", ");
                coordY.append(", ");

                }

                coordX.append(QString("%1").arg(i));    //voegt de x waarde van de punten die zwart zijn toe aan de array
                coordY.append(QString("%1").arg(j));

                first=false;

                coordSize++;

            }
        }
    }

    coordX.append("]; //X produced by gui prog");
    coordY.append("]; //Y produced by gui prog");

    progStr.append(coordX +"\n"+ coordY+"\n"+"int coordSize = ");
    progStr.append(QString("%1").arg(coordSize));
    progStr.append(";\n");

}

void Sowing::enableJoystick()   //Check als radiobutton enabled is
{
    if (ui->radioButton->isChecked())
    {
        joy=true;
    }
    else joy=false;
}



//void Sowing::readFile()
//{
//    QFile file("Arduino.pde");
//    //... (open the file for reading, etc.)
//    QTextStream textStream(&file);
//    while (true)
//    {
//        QString line = textStream.readLine();
//        if (line.isNull())
//            break;
//        else
//            progStr.append(line);
//    }
//}

