#include <Arduino.h>

#include "drivers/indicators.h"
#include "drivers/step_a4950.h"
#include "osape/osap/osap.h"
#include "osape/osap/vport_ucbus_drop.h"

OSAP* osap = new OSAP("stepper motor drop");
VPort_UCBus_Drop* vPortUcBusDrop = new VPort_UCBus_Drop();

// clank cz:
// AXIS   SPU     INVERT
// X:     320     false
// YL:    320     true
// YR:    320     false
// Z:     924.4r  false
// E:     830     currently false, not sure  
// per bondtech, for BMG on 16 microsteps, do 415: we are 32 microsteps 
// https://www.bondtech.se/en/customer-service/faq/ 

#define AXIS_PICK 2   // E: 3 Z: 2, Y: 1, X: 0
#define AXIS_INVERT false  
#define SPU 924.4445F 
#define C_SCALE 0.6F // 0-1, floating
#define TICKS_PER_PACKET 25.0F

void setup() {
  ERRLIGHT_SETUP;
  CLKLIGHT_SETUP;
  DEBUG1PIN_SETUP;
  DEBUG2PIN_SETUP;
  DEBUG3PIN_SETUP;
  DEBUG4PIN_SETUP;
  // osap
  osap->description = "remote stepper drop";
  osap->addVPort(vPortUcBusDrop);
  // stepper
  stepper_hw->init(AXIS_INVERT, C_SCALE);
}

// have available, 
// stepper_hw->setCurrent(currentChunks[AXIS_PICK].f);

void loop() {
  osap->loop();
  stepper_hw->dacRefresh();
} // end loop 


volatile float current_floating_pos = 0.0F;
volatile int32_t current_step_pos = 0;
volatile uint32_t delta_steps = 0;

volatile float vel = 0.0F; 
volatile float move_counter = 0.0F;

volatile boolean setBlock = false;


void UCBus_Drop::onPacketARx(void){
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  // last move is done, convert back steps -> float,
  current_floating_pos = current_step_pos / SPU;
  vel = 0.0F; // reset zero in case packet is not move 
  uint8_t bptr = 0;
  // switch bus packet types 
  switch(inBufferA[0]){
    case UB_AK_GOTOPOS:
      {
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        /*
        chunk_float64 target = { 
          .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3],
                    inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }};
                    */
        float delta = target.f - current_floating_pos;
        // update,
        //move_counter = 0.0F; // shouldn't need this ? try deleting 
        // direction swop,
        if(delta > 0){
          stepper_hw->dir(true);
        } else {
          stepper_hw->dir(false);
        }
        // how many steps, 
        delta_steps = lroundf(abs(delta * SPU));
        // what speed 
        vel = abs(delta * SPU) / TICKS_PER_PACKET;
        // for str8 r8 
        /*
        if(delta_steps == 0){
          vel = 0.0F;
        } else {
          vel = abs(delta * SPU) / TICKS_PER_PACKET;
        }
        */
        break; // end gotopos 
      }
    case UB_AK_SETPOS:
      {
        // reqest is to set position, not go to it... 
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        float target_current_pos = target.f;
        int32_t target_current_steps = lroundf(target_current_pos * SPU);
        setBlock = true; // don't do step work while these are modified 
        current_floating_pos = target_current_pos;
        current_step_pos = target_current_steps;
        vel = 0;
        delta_steps = 0;
        setBlock = false;
        break;
      }
    default:
      break;
  }  
  //DEBUG2PIN_OFF;
}

void UCBus_Drop::onRxISR(void){
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  move_counter += vel;
  boolean move_check = (move_counter > 1.0F);
  //DEBUG2PIN_TOGGLE;
  if(move_check){
    move_counter -= 1.0F;
    if(delta_steps == 0){
      // nothing 
    } else {
      //DEBUG1PIN_TOGGLE;
      stepper_hw->step();
      delta_steps --;
      if(stepper_hw->getDir()){
        current_step_pos ++;
      } else {
        current_step_pos --;
      }
    }
  }
}

void OSAP::handleAppPacket(uint8_t *pck, uint16_t ptr, pckm_t* pckm){
  pckm->vpa->clear(pckm->location);
}