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

volatile byte aa[5]={};
volatile byte i,j;

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

  //------- Lectura de datos via monitor serie ----------
    String msg="";
    Serial.println("Entrar aa[0]: ");
    while(msg == "")
      {
        msg = readSerialMsg();
      }
    aa[0] = msg.toInt();

    msg = "";
    Serial.println("Entrar aa[1]: ");
    while(msg == "")
      {
        msg = readSerialMsg();
      }
    aa[1] = msg.toInt();

    msg = "";
    Serial.println("Entrar aa[2]: ");
    while(msg == "")
      {
        msg = readSerialMsg();
      }
    aa[2] = msg.toInt();

    msg = "";
    Serial.println("Entrar aa[3]: ");
    while(msg == "")
      {
        msg = readSerialMsg();
      }
    aa[3] = msg.toInt();

    msg = "";
    Serial.println("Entrar aa[4]: ");
    while(msg == "")
      {
        msg = readSerialMsg();
      }
    aa[4] = msg.toInt();

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

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

  //------------- Proceso en alto nivel -------------------
  //aa[1]=aa[j]+aa[i];
  asm(
    "lds r30,i \n"
    "clr r31 \n" // Z <---- i en 16 bits
    "subi r30,lo8(-(aa)) \n"
    "sbci r31,hi8(-(aa)) \n" //Z <---- (dir aa[0])+i <----- aa[i]
    "ld r17,Z \n" // r17 <--- aa[i]
    "lds r30,j \n"
    "clr r31 \n" // Z <---- j en 16 bits
    "subi r30,lo8(-(aa)) \n"
    "sbci r31,hi8(-(aa)) \n" //Z <---- (dir aa[0])+j <----- aa[j]
    "ld r16,Z \n" // r16 <--- aa[j]
    "add r16,r17 \n" // r16 <----- aa[j]+aa[i]
    "sts aa+1,r16 \n" // aa[1] <--- aa[j]+aa[i] (son datos de tipo byte)
    ::: "r16","r17","r30","r31"
  ); 
  //------------------------------------------------------
  
  for(int j=0;j<5;j++)
  {
    Serial.print("a[");
    Serial.print(j);
    Serial.print("] = ");
    Serial.println(aa[j]);
  }


  DUMP(i);
  DUMP(j);
}
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 "";
}