//#include "AVRStepperPins.h"


#ifndef ARDUINO_ARCH_ESP32
  #error "Select an ESP32 board"
#endif
// These define's must be placed at the beginning before #include "_TIMERINTERRUPT_LOGLEVEL_.h"
// _TIMERINTERRUPT_LOGLEVEL_ from 0 to 4
#define _TIMERINTERRUPT_LOGLEVEL_     3
//----------------------------------------------------------------------------------------
//   Include files
//----------------------------------------------------------------------------------------

//#include <ACAN_ESP32.h>
#include <esp_chip_info.h>
#include <esp_flash.h>
#include <core_version.h> // For ARDUINO_ESP32_RELEASE
//#include "ESP32TimerInterrupt.h"
#include <esp_task_wdt.h>
#include "FastAccelStepper.h"
#include <DebugLog.h>

#define VERSION "post-4f2e1e4". //version of FastAccelStepper
//----------------------------------------------------------------------------------------
//   Data structures
//----------------------------------------------------------------------------------------

//constants
#define EN_PIN 3                //Define the enable signal port, filling with dummy for index 0 as motors have index starting at 1
#define STP_PIN 2               //Define the pulse signal port
#define DIR_PIN 0             //Define the direction signal port
#define ENDSTOP_PIN 6           //Define the endstop input port (incl. pullup)
#define LED_PIN 8               //Define internal LED pin port

#define slaveID 0x11            // CAN ID of node
#define maxparRx 4              // max number of receive parameters
#define maxparTx 4              // max number of send parameters
const uint8_t led_pin = 8;      //Define internal LED pin port ESP32 C3
#define MAX_STEPPER 1           // this is a one-stepper board
#define CW 1                    // CW means pos increases
#define CCW 0                   // CCW means position decreases
#define MAXMESSAGE 20           // max lenth of error message

//variables

struct structcmdDB {
  uint8_t cmd;                 // command ID
  int lenRx;                   // length of payload (excl CRC) = 1 to 7
  int nparamRx;                // total number of prameters to receive
  int nstartRx[maxparRx];      // start of MSB (note: numbering 7 6 5 ...1 0 15 14 ...9 8 23 ..)
  int nbitRx[maxparRx];        // total of bits for parameter
  bool bsignRx[maxparRx];      // is param signed (true) or unsigned (flase)
  int nscaleRx[maxparRx];      // scaler for floating numbers
  uint8_t actiononRx;          // action index 
  int lenTx;                   // length of payload (excl CRC) = 1 to 7
  int nparamTx;                // total number of prameters to transmit
  int nstartTx[maxparTx];      // start of MSB (see note numberin)
  int nbitTx[maxparTx];        // total of bits for parameter
//  bool bsignTx[maxparTx];      // is param signed (true) or unsigned (flase) -> not used
  int nscaleTx[maxparTx];      // scaler for floating numbers
} ;

structcmdDB cmdDB[4] = {
  {0x01, 1, 0, {}, {}, {}, {}, 1, 2, 1, {0}    ,{8}    ,{1}    }, // Ping, confirm 1 = ok, 0 = false
  {0x02, 1, 0, {}, {}, {}, {}, 1, 3, 2, {0,8}  ,{8,8}  ,{1,1}  }, // test, confirm 1 = ok, 0 = false
  {0x03, 1, 0, {}, {}, {}, {}, 1, 3, 3, {0,4,8},{4,4,8},{1,1,1}}, // test, confirm 1 = ok, 0 = false
  {0x00}
};

struct structcmdaction {
  uint8_t actiontxcmd;    // action id related to command
  uint8_t onreceipt;      // what to do on receipt of command, 1: return 1 (OK),return 2 (noted)
  uint8_t onfail;         // what if failed
  uint8_t onOK;           // what if command executed 
  bool returnvalue;       // true if main controller expects value(s) back
};

structcmdaction cmdaction[2] = {
  {0x01,0x01, 0x00, 0xFF, false}, // ping confirm receipt
  {0x00}
};

union Uint64on8 {                 // union to map 64bit to 8x 8bit
  uint64_t data;
  struct {
    uint8_t bdata[8];
  };
};
//----------------------------------------------------------------------------------------
//   Stepper 
//----------------------------------------------------------------------------------------

struct stepper_config_s {
  uint8_t step;
  uint8_t enable_low_active;
  uint8_t enable_high_active;
  uint8_t direction;
  uint16_t dir_change_delay;
  bool direction_high_count_up;
  bool auto_enable;
  uint32_t on_delay_us;
  uint16_t off_delay_ms;
};

const struct stepper_config_s stepper_config[MAX_STEPPER] = {
    {
      step : 2,
      enable_low_active : PIN_UNDEFINED,
      enable_high_active : 6,
      direction : 0,
      dir_change_delay : 0,
      direction_high_count_up : true,
      auto_enable : false,
      on_delay_us : 50,
      off_delay_ms : 1000
    }
};

//----------------------------------------------------------------------------------------
//. Robot node
//  parameters
//----------------------------------------------------------------------------------------
// homing
int16_t cwstop = 0;                 // position vs zero if homing CW
int16_t ccwstop = 0;                // position vs zero if homing CCW
uint8_t stopbackoff = 10;           // backoff when hitting home
uint16_t stopgap = 100;             // gap between left approach and right approach home switch
uint8_t homedir = CW;               // direction to move to home from sleep position
int16_t posmax = 0;                 // max position vs zero
int16_t posmin = 0;                 // min pos vs zero

//speed




enum { booth, config, home, idle, move, go2sleep, shutdown, error } mode = booth;
enum { none, warning, warningandinform, stopandinform, stopfatal} errorlevel = none;

//----------------------------------------------------------------------------------------
//. Stepper
//  creators for stepper
//----------------------------------------------------------------------------------------

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper[MAX_STEPPER];

//----------------------------------------------------------------------------------------
//. CAN functions
//  ESP32 Desired Bit Rate
//----------------------------------------------------------------------------------------

static const uint32_t DESIRED_BIT_RATE = 1000UL * 250UL ; // 250 kb/s

//----------------------------------------------------------------------------------------
//  CAN Support functions send and receive
//----------------------------------------------------------------------------------------

//CAN communication
uint rxCnt = 0;             //Receive data count
uint txCnt = 0;             //Receive data count
unsigned long msgTime = 0;

/*-----------------------------------------------------------------------------------------
Function: Calculate the checksum of a set of data
Input: buffer data to be verified
        size The number of data to be verified
output: checksum
*/
uint8_t getCheckSum(uint8_t id, uint8_t *buffer,uint8_t size){
  uint8_t i;
  uint16_t sum=id;
  for(i=0;i<size;i++)
    {
      sum += buffer[i];  //Calculate accumulated value
    }

  return(sum&0xFF);     //return checksum
} //getCheckSum

/*-----------------------------------------------------------------------------------------
returnmsg generic function makes message to send via CAN
parameters:
 slaveAddr: CAN ID of the slave (0 for broadcast to all slaves)
 cmd: command message
 param[]: parameters for cmd
*/
  bool returnmsg(uint8_t slaveAddr, uint8_t cmd, int param[maxparTx]) { 
  uint64_t dataTx = 0;
  uint64_t tmpdata;
  uint64_t mask;  
  union Uint64on8 bufdata;
  int cmdidx;
  int offset;
  
  CANMessage frame ;
  bool retVal = false;       //return value
  int i = 0;
  while ((cmdDB[i].cmd != cmd) && (cmdDB[i].cmd != 0x00)) {
   i++;
  }
  if (cmdDB[i].cmd == 0x00) {
    PRINTLN("Cmd ",cmd, " not found. Ignore");
    return(retVal);
  }
  cmdidx = i;

  PRINT("returnmsg: slave =",slaveAddr, ", cmd=", cmd,", param= ");
  for (int i = 0;i<cmdDB[cmdidx].nparamTx;i++) {
    PRINT(param[i], ", ");
  }
  PRINTLN("<-");
  dataTx = 0; 

  for (int i = 0; i < cmdDB[cmdidx].nparamTx;i++) {
    mask = pow(2,cmdDB[cmdidx].nbitTx[i])-1;
    tmpdata = (int64_t) param[i]& mask;                            
    offset = cmdDB[cmdidx].nstartTx[i]+ cmdDB[cmdidx].nbitTx[i]-1;
    dataTx = dataTx | tmpdata<<(8+ int(1+offset/8)*8 - offset%8-1);

  }
  bufdata.data = dataTx;
  bufdata.bdata[0] = cmd;

  frame.id =slaveAddr;
  frame.ext = false ;
  frame.rtr = false ;
  frame.len = cmdDB[cmdidx].lenTx +1;
  bufdata.bdata[cmdDB[cmdidx].lenTx] = getCheckSum(frame.id, bufdata.bdata,cmdDB[cmdidx].lenTx);
  for(int i = 0;i<=frame.len;i++){
    frame.data[i] = bufdata.bdata[i] ;
  }

  msgTime = micros();
  const bool ok = ACAN_ESP32::can.tryToSend (frame) ;

  if (ok) {
    txCnt += 1 ;
    retVal = true;
  }
  else {
    PRINT("Failed to reply command: ",cmd);
  }

  return(retVal);
}
/*-----------------------------------------------------------------------------------------
Function: wait for the slave to answer, set the timeout to 3000ms
Input: len Length of the response, i.e number of data fields
output:
   run successfully true
   failed to run false
   timeout no response false
*/
int CheckMsg( ) {

  int retVal = 0;                  //return value
  unsigned long sTime = millis();       //timing start time
  unsigned long time;                   //current moment
  uint8_t msgaction;
  uint8_t cmd;
  int param[4] = {0,0,0,0};

  CANMessage framerx ;

//  sTime = millis();    //get the current moment
//  resetRxBuffer();
//  PRINT(".");
  while (ACAN_ESP32::can.receive (framerx)) {
    PRINT("Return Msg: ",  micros()-msgTime, ", ", framerx.id, ", ");
    for (int i = 0;i<=framerx.len;i++) {
      PRINT(framerx.data[i], ", ");
    }
    PRINTLN(); 
    if (framerx.id == slaveID) {
      cmd = framerx.data[0];
      int i = 0;
      while ((cmdDB[i].cmd != cmd) && (cmdDB[i].cmd != 0x00)) {
        i++;
      }
      if (cmdDB[i].cmd == 0x00) {
        PRINTLN("Cmd ",cmd, " not found. Ignore");
        return(retVal);
      }
      msgaction = cmdDB[i].actiononRx;
    
      int j = 0;
      while ((cmdaction[j].actiontxcmd != msgaction) && (cmdaction[j].actiontxcmd != 0x00)) {
        j++;
      }
      if (cmdaction[j].actiontxcmd == 0x00) {
        PRINTLN("Received msg action not defined ",msgaction, " not found. Ignore");
        return(retVal);
      }
      retVal = i;
      if (cmdaction[j].onreceipt == 1) {
        param[0] = 1;
        returnmsg(slaveID, cmd, param);
      }
    }

  }
  return(retVal);
}


//----------------------------------------------------------------------------------------
//  Setup
//----------------------------------------------------------------------------------------

void setup() {
  int param[4] = {0,0,0,0};
//--- Switch on builtin led
  pinMode (LED_BUILTIN, OUTPUT) ;
  digitalWrite (LED_BUILTIN, HIGH) ;
//--- Start serial
  Serial.begin(115200);
  while (!Serial) {
    ;  // wait for USB serial port to connect.
  }

  delay (1000) ;
//--- Display ESP32 Chip Info
  uint32_t size_flash_chip ;
  esp_chip_info_t chip_info ;
  esp_chip_info (&chip_info) ;
  esp_flash_get_size (NULL, &size_flash_chip) ;
  PRINTLN("ESP32 Arduino Release: ", ARDUINO_ESP32_RELEASE) ;
  PRINTLN("ESP32 Chip Revision: ", chip_info.revision) ;
  PRINTLN("ESP32 SDK: ", ESP.getSdkVersion ()) ;
  PRINTLN("ESP32 Flash: ", size_flash_chip / (1024 * 1024)) ;
  PRINTLN(" MB ",((chip_info.features & CHIP_FEATURE_EMB_FLASH) != 0) ? "(embeded)" : "(external)") ;
  PRINTLN("APB CLOCK: ", APB_CLK_FREQ," Hz") ;
//--- Configure stepper
  pinMode(EN_PIN, OUTPUT);    //Set the enable signal port to output mode
  pinMode(STP_PIN, OUTPUT);   //Set the pulse signal port to output mode
  pinMode(DIR_PIN, OUTPUT);   //Set the direction signal port to output mode
  pinMode(ENDSTOP_PIN, INPUT_PULLUP);   //Set the direction signal port to output mode

  digitalWrite(EN_PIN, LOW);   //enable motor
  digitalWrite(STP_PIN, LOW);   //enable motor
  digitalWrite(DIR_PIN, HIGH); //output direction signal

//--- Configure ESP32 CAN
  PRINTLN("Configure ESP32 CAN") ;
  ACAN_ESP32_Settings settings (DESIRED_BIT_RATE) ;
//  settings.mRequestedCANMode = ACAN_ESP32_Settings::LoopBackMode ;
//  settings.mRxPin = GPIO_NUM_4 ; // Optional, default Tx pin is GPIO_NUM_4
//  settings.mTxPin = GPIO_NUM_5 ; // Optional, default Rx pin is GPIO_NUM_5
  const uint32_t errorCode = ACAN_ESP32::can.begin (settings) ;
  if (errorCode == 0) {
    PRINTLN("Bit Rate prescaler: ",settings.mBitRatePrescaler) ;
    PRINTLN("Time Segment 1:     ",settings.mTimeSegment1) ;
    PRINTLN("Time Segment 2:     ",settings.mTimeSegment2) ;
    PRINTLN("RJW:                ",settings.mRJW) ;
    PRINTLN("Triple Sampling:    ",settings.mTripleSampling ? "yes" : "no") ;
    PRINTLN("Actual bit rate:    ",settings.actualBitRate ()," bit/s") ;
    PRINTLN("Exact bit rate ?    ",settings.exactBitRate () ? "yes" : "no") ;
    PRINTLN("Distance            ",settings.ppmFromDesiredBitRate ()," ppm") ;
    PRINTLN("Sample point xx:    ",settings.samplePointFromBitStart (),"%") ;
    PRINTLN("Configuration OK!");
  }else {
    PRINTLN("Configuration error 0x", errorCode, HEX) ;
  }


//  PRINTLN("Stepper Version: ",MSG_STEPPER_VERSION);
  PRINTLN("    F_CPU=",F_CPU);
  PRINTLN("    TICKS_PER_S=",TICKS_PER_S);

  // If you are not sure, that the stepper hardware is working,
  // then try first direct port manipulation and uncomment the next line.
  // Alternatively use e.g. M1 T by serial command
  // test_direct_drive(&stepper_config[0]);

  engine.init();
  if (led_pin != PIN_UNDEFINED) {
    engine.setDebugLed(led_pin);
  }

  for (uint8_t i = 0; i < MAX_STEPPER; i++) {
    FastAccelStepper *s = NULL;
    const struct stepper_config_s *config = &stepper_config[i];
    if (config->step != PIN_UNDEFINED) {
      s = engine.stepperConnectToPin(config->step);
      if (s) {
        s->setDirectionPin(config->direction, config->direction_high_count_up,
                           config->dir_change_delay);
        s->setEnablePin(config->enable_low_active, true);
        s->setEnablePin(config->enable_high_active, false);
        s->setAutoEnable(config->auto_enable);
        s->setDelayToEnable(config->on_delay_us);
        s->setDelayToDisable(config->off_delay_ms);
      }
    }
    stepper[i] = s;
  }
}
void onError(char message[MAXMESSAGE], uint16_t error ) {

}
//----------------------------------------------------------------------------------------
//  Setup
//----------------------------------------------------------------------------------------

void loop() {
  if (mode == booth) {

  } 
  else if (mode == config) {
    
  } 
  else if (mode == home) {
    
  } 
  else if (mode == idle) {
    
  } 
  else if (mode == move) {
    
  }   
  else if (mode == go2sleep) {
    
  } 
  else if (mode == shutdown) {
    
  } 
  else if (mode == error) {
    
  } 
  else {
    PRINTLN("unknown mode ", mode);
  }
}