// bei Dataoverflow (=Timer muss gestoppt werden) geht LED auf D13 an. 
// 8 Bit serieller Empfang


// PortF D90-D97 für STEP 
// PortK D82-D89 für DIR
// PORTL D35-D42 für ENABLE
#define PORTSTEP PORTF
#define PORTDIR  PORTK
#define PORTEN   PORTL

#define OVF_LED 13

#define BUFSZ 1024

// Kommandopuffer
#define CMDLEN 16
#define CMD_OFFS 1
#define ARG_OFFS 2
#define AXES 8

// Timerfreq nit 1 Byte zwischen 500Hz und 127kHz linear regelbar. Mit BUF_FACT 4 sind das bei 0.0125mm/step 1.56-400mm/s
#define CPU 16000000L
#define BUF_FACT 4
#define SPEEDBASE CPU/500
// Defaultwert 32 ergibt den OCR1A Countwert 16M/500/32=1000 bzw. 500*32=16kHz 
#define DEF_SPEED 32
#define SPEED SPEEDBASE/DEF_SPEED

// Commands:
// 0x3 eb Eb                 // enable 
// 0x3 fb Fb                 // feedrate b * 125 
// 0xB gbbbbbbbbb Gbbbbbbbbb // 10 Byte: dir + 8 Microsteps 
 
// Kommandolängen
#define XLEN 3
#define GLEN 11
//

// Pufferzeugs
struct microSteps{
  byte dir, buf[BUFSZ];
  int size;  
};
typedef struct microSteps buffer; 
buffer buf0, buf1, *prepbuf = &buf0, *execbuf = &buf1;

// Kommandoschnittstelle
char cmdbuf[CMDLEN] ;    // Puffer für Kommandos max 12 bytes
unsigned int speed = SPEED; 

// Kontrolle
bool timer_stopped = true; 
int exec_cnt = 0;   // Ausgabezähler von Timer verwendet
bool buffer_full = false; 


//////////////////////////////////////////////////////////////////////////////////////
void setup() {
  pinMode(OVF_LED, OUTPUT);   // Overflow LED
  for(byte i = 82; i<98; ++i)  // PortF, PortK
    pinMode(i, OUTPUT); 
  for(byte i = 35; i<42; ++i)  // PortL
    pinMode(i, OUTPUT); 
  Serial.begin(115200); 
  cmdbuf[0] = 12; 
  cmdbuf[1] = 0; 
  cmdbuf[2] = 255; 
  cmdbuf[3] = 225; 
  cmdbuf[4] = 255; 
  cmdbuf[5] = 255; 
  cmdbuf[6] = 255; 
  cmdbuf[7] = 255; 
  cmdbuf[8] = 255; 
  cmdbuf[9] = 255;
  unsigned long  time = millis(); 
  prepBuf(); 
  Serial.println(millis()-time);
  printbuffer(prepbuf);
  
}

void loop() {
  delay(1000);
//  doCmd();     // führt Kommando aus, Wdh., wenn Timer nicht fertig 
}


void prepBuf(){
  buffer* Buf = prepbuf;

  // Puffer auf NULL initialisiern
  for(int i=0; i<BUFSZ; ++i) 
     Buf->buf[i]=0;

  // Pufferlänge ermitteln
  byte val; 
  byte axes = min(AXES,cmdbuf[0]-ARG_OFFS);
  byte max = 0;               
  for(byte axis=0; axis<axes; ++axis){
    val = cmdbuf[ARG_OFFS+axis];
    if(val>max) 
      max = val;
  }   
  
  if(max == 0){               // Leerer Frame? Dürfte nicht passieren, wird ignoriert. 
    err("Frame empty");
    return; 
  }

  Buf->size = BUF_FACT*max;     // Pufferlänge trimmen, damit bleibt Geschwindigkeit gleich 
  Buf->dir = cmdbuf[ARG_OFFS];  // Richtung setzen
  byte* buf = Buf->buf;
  unsigned int inc; 
  for(byte axis=0; axis<axes; ++axis)           // Puffer mit Step-Bits für alle Achsen bevölkern
  {
    val = cmdbuf[axis+ARG_OFFS];              // Schritte
    if(val==0) continue;                        //   

    inc = (Buf->size)*32/val;         // Abstand 
    byte bit = 1<<axis;
    for(unsigned int pos=inc/2; val--; pos+=inc)       //  verteile val Bits gleichmäßig im Puffer zwischen 0 und size
      buf[pos/32] |= bit;
  }

  buffer_full = true; // Puffer als fertig markieren
}

void printbuffer(buffer* Buf){
    for(int i=0;i<Buf->size;i++){
   Serial.print(i);
   Serial.print(" ");  
    for(int b=0;b<8;b++) 
      if(Buf->buf[i] & (1<<b))
        Serial.print("1");
      else
        Serial.print("0");
    Serial.println();
  }
}

void err(char* cp){
  Serial.print("Err: "); 
  Serial.println(cp); 
}