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

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("Entrar dato1: ");
  while(msg=="")
  {
    msg=readSerialMsg();
  }
  d1=msg.toInt();

  Serial.println("Entrar dato2: ");
  msg="";
  while(msg=="")
  {
    msg=readSerialMsg();
  }
  d2=msg.toInt();

  Serial.println("Entrar dato3: ");
  msg="";
  while(msg=="")
  {
    msg=readSerialMsg();
  }
  d3=msg.toInt();

  Serial.println("Entrar dato4: ");
  msg="";
  while(msg=="")
  {
    msg=readSerialMsg();
  }
  d4=msg.toInt();

  Serial.println("Entrar dato5: ");
  msg="";
  while(msg=="")
  {
    msg=readSerialMsg();
  }
  d5=msg.toInt();

  //------------- Proceso en assembly -------------------
  /*v1=d2+d3;
  if(v1==d1)
  {
    v2=v1-d4;
    v3=v2+d5;
  }
  else
  {
    v2=v1-d5;
    v3=v2+d4;
  }*/

  asm(
    "lds r1,d1 \n"
    "lds r2,d2 \n"
    "lds r3,d3 \n"
    "lds r4,d4 \n"
    "lds r5,d5 \n"
    //---------------
    "add r6,r2 \n" // r6 <-- v1
    "add r6,r3 \n"
    "sts v1,r6 \n"
    //---------------
    "cp r6,r1 \n"
    "breq TRUECASE \n" // v1 - d1 (Z=1)
    "rjmp FALSECASE \n"
    //---------------
    "TRUECASE:\n"
    "add r7,r6 \n" // r7 <-- v2
    "sub r7,r4 \n"
    "sts v2,r7 \n"
    "add r8,r7 \n" // r8 <-- v3
    "add r8,r5 \n"
    "sts v3,r8 \n"
    "rjmp COMMONCODE \n"
    //---------------
    "FALSECASE:\n"
    "add r7,r6 \n" // r7 <-- v2
    "sub r7,r5 \n"
    "sts v2,r7 \n" 
    "add r8,r7 \n" // r8 <-- v3
    "add r8,r4 \n"
    "sts v3,r8 \n"
    //---------------
    "COMMONCODE:\n"
    ::: "r1","r2","r3","r4","r5","r6","r7","r8"
  );
  //------------------------------------------------------
  
  DUMP(v1);
  DUMP(v1);
  DUMP(v2);
  DUMP(v3);
}
void loop() {

}

//----------- lectura de string enviada via monitor serie -----//
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 "";
}