// Tmc2209.c
#include "Tmc2209.h"


#define portMAX_DELAY 10
#define UART_BUFFER_SIZE 64 // Taille du tampon UART, à ajuster selon vos besoins

// Adresses des registres
#define GCONF       0x00
#define GSTAT       0x01
#define IFCNT       0x02
#define IOIN        0x06
#define IHOLD_IRUN  0x10
#define TSTEP       0x12
#define TCOOLTHRS   0x14
#define SGTHRS      0x40
#define SG_RESULT   0x41
#define MSCNT       0x6A
#define CHOPCONF    0x6C
#define DRVSTATUS   0x6F
#define VACTUAL     0x22

// Bits associés à GCONF
#define I_SCALE_ANALOG  (1 << 0)
#define INTERNAL_RSENSE (1 << 1)
#define EN_SPREADCYCLE  (1 << 2)
#define SHAFT           (1 << 3)
#define INDEX_OTPW      (1 << 4)
#define INDEX_STEP      (1 << 5)
#define MSTEP_REG_SELECT (1 << 7)

// Bits associés à GSTAT
#define RESET       (1 << 0)
#define DRV_ERR     (1 << 1)
#define UV_CP       (1 << 2)

// Bits associés à CHOPCONF
#define VSENSE      (1 << 17)
#define MSRES0      (1 << 24)
#define MSRES1      (1 << 25)
#define MSRES2      (1 << 26)
#define MSRES3      (1 << 27)
#define INTPOL      (1 << 28)

// Bits associés à IOIN
#define IO_ENN      (1 << 0)
#define IO_STEP     (1 << 7)
#define IO_SPREAD   (1 << 8)
#define IO_DIR      (1 << 9)

// Bits associés à DRVSTATUS
#define STST        (1U << 31)
#define STEALTH     (1U << 30)
#define CS_ACTUAL   (31U << 16)
#define T157        (1 << 11)
#define T150        (1 << 10)
#define T143        (1 << 9)
#define T120        (1 << 8)
#define OLB         (1 << 7)
#define OLA         (1 << 6)
#define S2VSB       (1 << 5)
#define S2VSA       (1 << 4)
#define S2GB        (1 << 3)
#define S2GA        (1 << 2)
#define OT          (1 << 1)
#define OTPW        (1 << 0)

// Bits associés à IHOLD_IRUN
#define IHOLD       (31 << 0)
#define IRUN        (31 << 8)
#define IHOLDDELAY  (15 << 16)

// Bits associés à SGTHRS
#define SGTHRS_VAL  (255 << 0)

// Autres constantes
#define MRES_256    0
#define MRES_128    1
#define MRES_64     2
#define MRES_32     3
#define MRES_16     4
#define MRES_8      5
#define MRES_4      6
#define MRES_2      7
#define MRES_1      8




void TMC2209_Init(TMC2209* tmc2209,TMC_UART* serial_instance) {

    tmc2209->serial_instance = serial_instance;

    tmc2209->mtr_id = 0;
    tmc2209->rFrame[0] = 0x55;
    tmc2209->rFrame[1] = 0;
    tmc2209->rFrame[2] = 0;
    tmc2209->rFrame[3] = 0;
    tmc2209->wFrame[0] = 0x55;
    tmc2209->wFrame[1] = 0;
    tmc2209->wFrame[2] = 0;
    tmc2209->wFrame[3] = 0;
    tmc2209->wFrame[4] = 0;
    tmc2209->wFrame[5] = 0;
    tmc2209->wFrame[6] = 0;
    tmc2209->wFrame[7] = 0;
    tmc2209->result[0] = 0;
    tmc2209->result[1] = 0;
    tmc2209->result[2] = 0;
    tmc2209->result[3] = 0;

    // return true;
}

uint8_t compute_crc8_atm(const uint8_t *datagram, size_t length, uint8_t initial_value) {
    uint8_t crc = initial_value;
    
    size_t i;
    int j;
    for (i = 0; i < length; i++) {
        uint8_t byte = datagram[i];
        for (j = 0; j < 8; j++) {
            if ((crc >> 7) ^ (byte & 0x01)) {
                crc = ((crc << 1) ^ 0x07) & 0xFF;
            } else {
                crc = (crc << 1) & 0xFF;
            }
            byte >>= 1;
        }
    }
    return crc;
}



uint64_t *read_reg(TMC2209* tmc2209, int reg) {

    tmc2209->rFrame[0] = 0x55;
    tmc2209->rFrame[1] = tmc2209->mtr_id;
    tmc2209->rFrame[2] = reg;
    tmc2209->rFrame[3] = compute_crc8_atm(tmc2209->rFrame, 3, 0); 

    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[0]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[1]);
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[2]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[3]);

    uint64_t temp_result[12] = {0x00};

    int i = 0;
    uint32_t start_time = time_us_32(); // Temps de départ en microsecondes
    while (time_us_32() - start_time < 2000 && i < 12) { // Lire pendant 2 ms
        if (uart_is_readable(tmc2209->serial_instance->UART_ID)) {
            uint64_t tempchar =  uart_getc(tmc2209->serial_instance->UART_ID);
            temp_result[i] = tempchar;
            i++;
        }
    }
    
    if (temp_result[11] == 0 ){
        tmc2209->result[0] = temp_result[6];
        tmc2209->result[1] = temp_result[7];
        tmc2209->result[2] = temp_result[8];
        tmc2209->result[3] = temp_result[9];
    }else{
        tmc2209->result[0] = temp_result[7];
        tmc2209->result[1] = temp_result[8];
        tmc2209->result[2] = temp_result[9];
        tmc2209->result[3] = temp_result[10];
    }

    sleep_ms(3);
    return tmc2209->result; 
}


uint32_t read_int(TMC2209* tmc2209, int reg) {
  uint8_t data[4]; // Array to store the 4-byte response

  for (int tries = 0; tries < 10; tries++) {
    uint64_t* rtn = read_reg(tmc2209, reg);

    if (rtn == NULL) {
      printf("TMC2209: read_reg failed to return data\n");
      continue;
    }

    if (sizeof(*rtn) >= sizeof(data)) {
        for (int i = 0; i < sizeof(data); i++) {
            data[i] = (uint8_t)(*rtn >> (8 * (sizeof(*rtn) - i - 1)));
        }
    } else {
        printf("TMC2209: expected at least 4 bytes, got %zu Bytes\n", sizeof(*rtn));
        continue;
    }

    uint32_t result_int = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];

    if (!(result_int & 0x80000000)) { 
        return result_int; 
    } else {
      printf("TMC2209: error in register response: 0x%08X\n", result_int);
    }
  }

  printf("TMC2209: failed to read register after 10 tries\n");
  printf("TMC2209: is Stepper Powersupply switched on?\n");
  exit(EXIT_FAILURE);
}


bool write_reg(TMC2209* tmc2209, int reg, int val) {
    tmc2209->wFrame[0] = 0x55;
    tmc2209->wFrame[1] = tmc2209->mtr_id;
    tmc2209->wFrame[2] = reg | 0x80; 
    tmc2209->wFrame[3] = (val >> 24) & 0xFF;
    tmc2209->wFrame[4] = (val >> 16) & 0xFF;
    tmc2209->wFrame[5] = (val >> 8) & 0xFF;
    tmc2209->wFrame[6] = val & 0xFF;
    tmc2209->wFrame[7] = compute_crc8_atm(tmc2209->wFrame, sizeof(tmc2209->wFrame) - 1, 0);
    // printf("val : %d\n",val);
    // printf("Frames :");
    // for (int i = 0; i < 8; i++) {
    //     printf("%d ", tmc2209->wFrame[i]);
    // }
    // printf("\n");

    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[0]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[1]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[2]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[3]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[4]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[5]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[6]); 
    uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[7]); 

    sleep_ms(3); 

    return true;
}

bool write_reg_check(TMC2209* tmc2209, int reg, int val) {

    int ifcnt12 = read_int(tmc2209, IFCNT);
    unsigned int ifcnt1 = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    write_reg(tmc2209, reg, val);

    int ifcnt22 = read_int(tmc2209, IFCNT);
    unsigned int ifcnt2 = (unsigned int)tmc2209->result[0] << 24 |
                    (unsigned int)tmc2209->result[1] << 16 |
                    (unsigned int)tmc2209->result[2] << 8 |
                    (unsigned int)tmc2209->result[3];

    if (ifcnt1 >= ifcnt2) {
        printf("TMC2209: writing not successful!\n");
        printf("reg: %X val: %d\n", reg, val);
        printf("ifcnt: %d %d\n", ifcnt1, ifcnt2);
        return false;
    }

    return true;
}

void TMC2209_destroy(TMC2209* tmc2209) {
    // free(tmc2209);
}





void driver_status(TMC2209* tmc2209) {
    printf("TMC2209: ---\n");
    printf("TMC2209: DRIVER STATUS:\n");
    read_int(tmc2209, DRVSTATUS);
    unsigned int drvstatus = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (drvstatus & STST) {
        printf("TMC2209: Info: motor is standing still\n");
    } else {
        printf("TMC2209: Info: motor is running\n");
    }

    if (drvstatus & STEALTH) {
        printf("TMC2209: Info: motor is running on StealthChop\n");
    } else {
        printf("TMC2209: Info: motor is running on SpreadCycle\n");
    }

    int cs_act = drvstatus & CS_ACTUAL;
    cs_act >>= 16;
    printf("TMC2209: CS actual: %d\n", cs_act);

    if (drvstatus & OLB) {
        printf("TMC2209: Warning: Open load detected on phase B\n");
    }
    if (drvstatus & OLA) {
        printf("TMC2209: Warning: Open load detected on phase A\n");
    }
    if (drvstatus & S2VSB) {
        printf("TMC2209: Error: Short on low-side MOSFET detected on phase B. The driver becomes disabled\n");
    }
    if (drvstatus & S2GA) {
        printf("TMC2209: Error: Short on low-side MOSFET detected on phase A. The driver becomes disabled\n");
    }
    if (drvstatus & S2GB) {
        printf("TMC2209: Error: Short to GND detected on phase B. The driver becomes disabled.\n");
    }
    if (drvstatus & S2GA) {
        printf("TMC2209: Error: Short to GND detected on phase A. The driver becomes disabled.\n");
    }
    if (drvstatus & OT) {
        printf("TMC2209: Error: Driver Overheating!\n");
    }
    if (drvstatus & OTPW) {
        printf("TMC2209: Warning: Driver Overheating Prewarning!\n");
    }
    printf("end");


}

// Fonction general_config
void general_config(TMC2209* tmc2209) {
    printf("TMC2209: ---\n");
    printf("TMC2209: GENERAL CONFIG\n");
    read_int(tmc2209, GCONF);
    unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];

    if (gconf & I_SCALE_ANALOG) {
        printf("TMC2209: Driver is using voltage supplied to VREF as current reference\n");
    } else {
        printf("TMC2209: Driver is using internal reference derived from 5VOUT\n");
    }
    
    if (gconf & INTERNAL_RSENSE) {
        printf("TMC2209: Internal sense resistors. Use current supplied into VREF as reference.\n");
        printf("TMC2209: VREF pin internally is driven to GND in this mode.\n");
        printf("TMC2209: This will most likely destroy your driver!!!\n");
        exit(EXIT_FAILURE);
    } else {
        printf("TMC2209: Operation with external sense resistors\n");
    }
    
    if (gconf & EN_SPREADCYCLE) {
        printf("TMC2209: SpreadCycle mode enabled\n");
    } else {
        printf("TMC2209: StealthChop PWM mode enabled\n");
    }
    
    if (gconf & SHAFT) {
        printf("TMC2209: Inverse motor direction\n");
    } else {
        printf("TMC2209: normal motor direction\n");
    }
     
    if (gconf & INDEX_OTPW) {
        printf("TMC2209: INDEX pin outputs overtemperature prewarning flag\n");
    } else {
        printf("TMC2209: INDEX shows the first microstep position of sequencer\n");
    }
    
    if (gconf & INDEX_STEP) {
        printf("TMC2209: INDEX output shows step pulses from internal pulse generator\n");
    } else {
        printf("TMC2209: INDEX output as selected by index_otpw\n");
    }
}

// Fonction general_stat
void general_stat(TMC2209* tmc2209) {
    printf("TMC2209: ---\n");
    printf("TMC2209: GENERAL STAT\n");
    read_int(tmc2209, GSTAT);
    unsigned int gstat = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (gstat & RESET) {
        printf("TMC2209: The Driver has been reset since the last read access to GSTAT\n");
    }
    
    if (gstat & DRV_ERR) {
        printf("TMC2209: The driver has been shut down due to overtemperature or short circuit detection since the last read access\n");
    }
    
    if (gstat & UV_CP) {
        printf("TMC2209: Undervoltage on the charge pump. The driver is disabled in this case\n");
    }
}

bool set_vactual(TMC2209* tmc2209, int value) {
    return write_reg_check(tmc2209, VACTUAL, value);
}



unsigned int set_bit(unsigned int value, unsigned int bit) {
    unsigned int temp = value | bit; 
    return temp;
}


// Fonction pour définir un bit spécifique à 0
unsigned int clear_bit(unsigned int value, unsigned int bit) {
    unsigned int temp = value & ~(1 << bit);
    return temp;
}


void clear_general_stat(TMC2209* tmc2209) {
    read_int(tmc2209, GSTAT);
    unsigned int gstat= (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    gstat = set_bit(gstat, RESET);
    gstat = set_bit(gstat, DRV_ERR);
    write_reg_check(tmc2209, GSTAT, gstat);
}



void set_voltage_sense(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, CHOPCONF); // Lire la valeur actuelle du registre CHOPCONF
    unsigned int chopconf= (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (enabled) {
        chopconf |= VSENSE;
    } else {
        chopconf &= ~VSENSE;
    }
    
    // Écrire la nouvelle valeur dans le registre CHOPCONF
    if (!write_reg_check(tmc2209, CHOPCONF, chopconf)) {
        printf("Erreur lors de l'écriture de la valeur dans le registre CHOPCONF\n");
    }
}

bool get_voltage_sense(TMC2209* tmc2209) {
    read_int(tmc2209, CHOPCONF);
    unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    return (chopconf & VSENSE) != 0;
}


void set_current_flow(TMC2209* tmc2209,double run_current, double hold_current_multiplier, int hold_current_delay, double Vref) {
    double CS_IRun = 0.0;
    double Rsense = 0.11;
    double Vfs = 0.0;
    Vref = 5.4;
    hold_current_multiplier = 0.5;
    hold_current_delay = 10;
    run_current = 1800;

    int voltage_sense = get_voltage_sense(tmc2209);

    if (voltage_sense) {
        Vfs = 0.180 * Vref / 2.5;
    } else {
        Vfs = 0.325 * Vref / 2.5;
    }

    CS_IRun = 32.0 * 1.41421 * run_current / 1000.0 * (Rsense + 0.02) / Vfs - 1;
    CS_IRun = fmin(CS_IRun, 31);
    CS_IRun = fmax(CS_IRun, 0);
    double CS_IHold = hold_current_multiplier * CS_IRun;
    CS_IRun = round(CS_IRun);
    CS_IHold = round(CS_IHold);

    // printf("CS_IHold_rounded  %.3f, CS_IRun_rounded  %.3f, hold_current_delay  %d\n" , CS_IHold, CS_IRun, hold_current_delay);

    set_irun_ihold(tmc2209,CS_IHold, CS_IRun, hold_current_delay);
}


void set_iscale_analog(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, GCONF);
    unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (enabled) {

        gconf = set_bit(gconf, I_SCALE_ANALOG);
    } else {

        gconf = clear_bit(gconf, I_SCALE_ANALOG);
    }

    write_reg_check(tmc2209, GCONF, gconf);
}

void set_interpolation(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, CHOPCONF);
    unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (enabled) {
        chopconf = set_bit(chopconf, INTPOL);
    } else {
        chopconf = clear_bit(chopconf, INTPOL);
    }


    printf("chopconf : %d\n",chopconf);

    write_reg_check(tmc2209, CHOPCONF, chopconf);
}

void set_internal_resistor_sense(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, GCONF);
    unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (enabled) {

        gconf = set_bit(gconf, INTERNAL_RSENSE);
    } else {

        gconf = clear_bit(gconf, INTERNAL_RSENSE);
    }

    write_reg_check(tmc2209, GCONF, gconf);
}

void set_spread_cycle(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, GCONF);
    unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    if (enabled) {

        gconf = set_bit(gconf, EN_SPREADCYCLE);
    } else {

        gconf = clear_bit(gconf, EN_SPREADCYCLE);
    }

    write_reg_check(tmc2209, GCONF, gconf);
}

void set_microstepping_resolution(TMC2209* tmc2209, int msres) {

    read_int(tmc2209, CHOPCONF);
    unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
                        (unsigned int)tmc2209->result[1] << 16 |
                        (unsigned int)tmc2209->result[2] << 8 |
                        (unsigned int)tmc2209->result[3];
    // printf("chopconf %d \n",chopconf);

    int msresdezimal = 8 - (int)log2(msres);
    chopconf &= 4043309055;
    chopconf |= msresdezimal << 24; 
    write_reg_check(tmc2209, CHOPCONF, chopconf);
    set_microstep_resolution_regselect(tmc2209, true);
}

int get_microstepping_resolution(TMC2209* tmc2209) {
    read_int(tmc2209, CHOPCONF);
    unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
                            (unsigned int)tmc2209->result[1] << 16 |
                            (unsigned int)tmc2209->result[2] << 8 |
                            (unsigned int)tmc2209->result[3];


    // printf("chopconf  : %d\n", chopconf);
    int msresdezimal = chopconf & (MSRES0 | MSRES1 | MSRES2 | MSRES3);
    msresdezimal = msresdezimal >> 24;
    msresdezimal = 8 - msresdezimal;
    int micro_stepping_resolution = (int)pow(2, msresdezimal);

    return micro_stepping_resolution;
}

void set_microstep_resolution_regselect(TMC2209* tmc2209, bool enabled) {
    read_int(tmc2209, GCONF);
    unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
                                (unsigned int)tmc2209->result[1] << 16 |
                                (unsigned int)tmc2209->result[2] << 8 |
                                (unsigned int)tmc2209->result[3];
    if (enabled) {
        gconf = set_bit(gconf, MSTEP_REG_SELECT);
    } else {
        gconf = clear_bit(gconf, MSTEP_REG_SELECT);
    }
    // printf("gconf : %d \n",gconf);

    write_reg_check(tmc2209, GCONF, gconf);
}


void set_irun_ihold(TMC2209* tmc2209, double IHold, double IRun, int IHoldDelay) {
    // Convertir les valeurs doubles en entiers
    int ihold_irun = 0;
    int ih = (int)IHold;
    int ir = (int)IRun;

    ihold_irun |= ih << 0;
    printf("ihold_irun1 : %d\n",ihold_irun);

    ihold_irun |= ir << 8;
    printf("ihold_irun2 : %d\n",ihold_irun);

    ihold_irun |= IHoldDelay << 16;

    printf("ihold_irun3 : %d\n",ihold_irun);

    write_reg_check(tmc2209, IHOLD_IRUN, ihold_irun);
}