#pragma GCC optimize ("Og")
#include "ArduinoTrace.h"

//volatile byte d1=84,d2=37,d3=47,d4=13,d5=8,v1,v2,v3; //prueba1
//volatile byte d1=84,d2=37,d3=46,d4=13,d5=8,v1,v2,v3; //prueba1


volatile byte d1,d2,d3,d4,d5,v1,v2,v3; 

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("INICIO");


  String msg = "";
  Serial.println("Ingrese dato 1: ");
  while(msg == ""){
    msg = readSerialMsg();
  }
  d1 = msg.toInt();

  msg = "";
  Serial.println("Ingrese dato 2: ");
  while(msg == ""){
    msg = readSerialMsg();
  }
  d2 = msg.toInt();

  msg = "";
  Serial.println("Ingrese dato 3: ");
  while(msg == ""){
    msg = readSerialMsg();
  }
  d3 = msg.toInt();

  msg = "";
  Serial.println("Ingrese dato 4: ");
  while(msg == ""){
    msg = readSerialMsg();
  }
  d4 = msg.toInt();


  msg = "";
  Serial.println("Ingrese dato 5: ");
  while(msg == ""){
    msg = readSerialMsg();
  }
  d5 = msg.toInt();




  //------------- Proceso en alto nivel -------------------
  //v1=d2+d3;
  //if(v1==d1){
  //  v2=v1-d4;
  //  v3=v2+d5;
  //}
  //else{
  //  v2=v1-d5;
  //  v3=v2+d4;
  //}
  //------------------------------------------------------
  asm(
    "lds r16, d2 \n"    
    "lds r17, d3 \n"    
    "lds r18, d1 \n"    
    "add r16, r17 \n"   //d2+d3
    "sts v1, r16 \n"    //v1 = d2 + d4
    "cp r16, r18 \n"    //r16-r18
    "breq L10 \n"       //Branch if r16=r18 (Z=1) ; equ ints.: brbs 1,L10
    "rjmp L30 \n"       //salto a L30

    "L10: \n"
    "lds r16, v1 \n"    
    "lds r17, d4 \n"    
    "clc \n"            //clear carry
    "sbc r16, r17 \n"   //v1-d4
    "sts v2, r16 \n"    //v2 = v1-d4
    "rjmp L20 \n"       //saltar a L20

    "L20: \n"
    "lds r16, v2 \n"    
    "lds r17, d5 \n"    
    "add r16, r17 \n"   //v2+d5
    "sts v3, r16 \n"    //v3 = v2+d5
    "rjmp L40 \n"

    "L30: \n"
    "lds r16, v1 \n"    
    "lds r17, d5 \n"    
    "clc \n"            //clear carry
    "sbc r16, r17 \n"   //v1-d5
    "sts v2, r16 \n"    //v2 = v1-d5

    "lds r16, v2 \n"    
    "lds r17, d4 \n"    
    "add r16, r17 \n"   //v2 + d4
    "sts v3, r16 \n"    //v3 = v2 + d4
    "rjmp L40 \n"

    "L40: \n"
    ::: "r16", "r17", "r18", "r19"

  );
  DUMP(v1);
  DUMP(v2);
  DUMP(v3);
}
void loop() {

}



String readSerialMsg()
{
  String message="";
  char incomingChar;
  while (Serial.available())
  {
    char incomingChar = Serial.read();
    if (incomingChar != '\n'){
      message += String(incomingChar);
    }
    else{
      return message;
    }
    delay(1);
  }
  return "";
}