// to test z axis to position 1 ,  send 98 41 1 99 
// to test z axis to position 2 ,  send 98 41 2 99 
// to test mixer at a speed of 20 , send 98 42 20 99 
// to test mixer at a speed of 100 , send 98 42 100 99

#include <SoftwareSerial.h>

#define step_command 2 // PA2 _ 2 in Arduino
#define dir 3 // PA3 _ 3 in Arduino
#define mixer 7 // PA7 _ 7 in Arduino
#define fin_de_course 8 // PB2 _ 2 ou 8

//TX , PA1 , Arduino 0
//RX , PA0 , Arduino A1

SoftwareSerial mySerial(A1, 0); // RX, TX

// address from 40 to 49
const byte code_start = 98;// code début : 98
const byte code_stop = 99;// code fin : 99
const byte code_confirm_receipt = 100; //100 accuse reception
const byte code_action_finished = 101; //101 fin d'operation
const byte code_ready_for_action = 102; //102 préparé
const byte adresse_chariot = 41;
const byte adresse_mixer = 42;

char received = 0;
byte decoded[3] = {1,2,3};

bool chariot_running = 0;
bool mixer_running = 0;

boolean fin_de_course_value;
int step_max = 1000;
int step_delay = 1000;

long chariot_position = 0;
long chariot_hauteur[4] = {1000,15000,20000,24000};
long chariot_arrivee = 0;
long nombre_de_pas;

int mixing_time = 10; // 10 seconds

void setup() {
  pinMode(step_command, OUTPUT);
  pinMode(dir, OUTPUT);
  pinMode(mixer, OUTPUT);
  pinMode(fin_de_course, INPUT);

  mySerial.begin(9600);

  fin_de_course_value = digitalRead(fin_de_course);

          // if the z axis is already touching, it is good to move it and make the initialisation
          if(fin_de_course_value == 1){
              digitalWrite(dir, LOW);
              for(int i = 0; i <4000 ;i++ ){
                digitalWrite(step_command, HIGH);
                delayMicroseconds(step_delay);
                digitalWrite(step_command, LOW);
              }
          }


  
  chariot_initialisation();

}

void loop() {

  lecture_reseau();

  if(decoded[0] == adresse_chariot && decoded[1] != 0){
        mySerial.write(code_start);mySerial.write(adresse_chariot);mySerial.write(code_confirm_receipt);mySerial.write(code_stop);
        chariot_mouvement(chariot_hauteur[decoded[1]]);
        mySerial.write(code_start);mySerial.write(adresse_chariot);mySerial.write(code_action_finished);mySerial.write(code_stop);

        reset_buffer();
  }
                    
  if(decoded[0] == adresse_mixer && decoded[1] != 0){
            //mySerial.print("mixer");mySerial.println(decoded[1]);
            mySerial.write(code_start);mySerial.write(adresse_mixer);mySerial.write(code_confirm_receipt);mySerial.write(code_stop);
            analogWrite(mixer, decoded[1]);
            delay(mixing_time * 1000); // temps de mixage
            analogWrite(mixer, 0);
            mySerial.write(code_start);mySerial.write(adresse_mixer);mySerial.write(code_action_finished);mySerial.write(code_stop);
            
            reset_buffer();
            chariot_initialisation(); // start again
  }

}

void lecture_reseau(){
    if (mySerial.available()) {    
              received = mySerial.read();
                          if(received == code_start){
                            //mySerial.println("code start");mySerial.print(received);
                            while (mySerial.available() == 0) {
                            delayMicroseconds(1);
                            }
                            if (mySerial.available()) {
                            decoded[0] = mySerial.read();
                            }
                            while (mySerial.available() == 0) {
                            delayMicroseconds(1);
                            }
                            if (mySerial.available()) {
                            decoded[1] = mySerial.read();
                            }
                            while (mySerial.available() == 0) {
                            delayMicroseconds(1);
                            }
                            if (mySerial.available()) {
                            decoded[2] = mySerial.read();

                                    if(decoded[2] != code_stop ){
                                      decoded[0] = 0;
                                      decoded[1] = 0;
                                      //mySerial.print("pas bien");

                                    }

                            }
                          }
  }
  
}

void reset_buffer(){
  decoded[0] = 0;
  decoded[1] = 0;
  //mySerial.println("reset buffer");
}

void chariot_initialisation(){
    fin_de_course_value = digitalRead(fin_de_course);
    digitalWrite(dir, HIGH);
    while(fin_de_course_value == 0){
        digitalWrite(step_command, HIGH);
        delayMicroseconds(step_delay);
        digitalWrite(step_command, LOW);
        fin_de_course_value = digitalRead(fin_de_course);
    }

    chariot_position = 0;
    //mySerial.print("fin de course");
    mySerial.write(code_start);mySerial.write(adresse_chariot);mySerial.write(code_ready_for_action);mySerial.write(code_stop);

}

void chariot_mouvement(long position_arrivee){

  nombre_de_pas = position_arrivee - chariot_position;

  //mySerial.print("nombre de pas a faire");mySerial.println(nombre_de_pas);
  
    digitalWrite(dir, LOW);
    for(int i = 0; i <nombre_de_pas ;i++ ){
        digitalWrite(step_command, HIGH);
        delayMicroseconds(step_delay);
        digitalWrite(step_command, LOW);
    }

    long chariot_position = position_arrivee;
    //mySerial.print("arrivee ");
    // envoi confirmation en position
    mySerial.write(code_start);mySerial.write(adresse_chariot);mySerial.write(code_ready_for_action);mySerial.write(code_stop);
    
}