#include "AS5600L.h"

void as560x_init(AS5600L* as5600l, i2c_inst_t *I2C_ID, int frq , int sda, int scl ) {
    i2c_init(I2C_ID, frq);
    gpio_set_function(sda, GPIO_FUNC_I2C);
    gpio_set_function(scl, GPIO_FUNC_I2C);
    gpio_pull_up(sda);
    gpio_pull_up(scl);

    AS5600L_I2C instance_i2c;
    instance_i2c.I2C_PORT = I2C_ID;
    instance_i2c.FREQUENCE = frq;
    instance_i2c.I2C_SDA_PIN = sda;
    instance_i2c.I2C_SCL_PIN = scl;

    as5600l->serial_instance = instance_i2c;
    as5600l->abs_pos = 0;
    as5600l->curr_angle = 0;
    as5600l->last_angle = 0;
    as5600l->start_pos = 0;
    

    as5600l->start_pos = as560xReadAngle(as5600l);
    as5600l->abs_pos = 0 - as5600l->start_pos;
}

int update_pos(AS5600L* as5600l){
    // sensorData(); 
    as5600l->curr_angle = as560xReadAngle(as5600l);

    if(as5600l->last_angle > ENCODER_RESOLUTION/2 & as5600l->curr_angle < (as5600l->last_angle - ENCODER_RESOLUTION/2)){
        as5600l->abs_pos = as5600l->abs_pos + ENCODER_RESOLUTION - as5600l->last_angle + as5600l->curr_angle;
    }else if(as5600l->curr_angle > ENCODER_RESOLUTION/2 &   as5600l->last_angle < (as5600l->curr_angle - ENCODER_RESOLUTION/2) ){
        as5600l->abs_pos = as5600l->abs_pos -ENCODER_RESOLUTION - as5600l->last_angle + as5600l->curr_angle;
    }else{
        as5600l->abs_pos = as5600l->abs_pos - as5600l->last_angle + as5600l->curr_angle;
    }
    
    as5600l->last_angle = as5600l->curr_angle;
    return as5600l->abs_pos;
}

int __bswap16(int value) {
    return (value >> 8) | (value << 8);
}

void i2cError() {
    printf("I2C Error\n");
}

int as560xReadReg(AS5600L* as5600l,int addr, bool wide, int mask) {
    int buf;
    int result = i2c_write_timeout_us(as5600l->serial_instance.I2C_PORT, AS5600L_ADDRESS, (uint8_t *)&addr, 1, true, I2C_TIMEOUT_US);
    if (result <= 0) {
        i2cError();
    }
    result = i2c_read_timeout_us(as5600l->serial_instance.I2C_PORT, AS5600L_ADDRESS, (uint8_t *)&buf, (wide ? 2 : 1), false, I2C_TIMEOUT_US);
    if (result <= 0) {
        i2cError();
    }
    if (wide) {
        return __bswap16(buf) & mask;
    } else {
        return buf & mask;
    }
}

void AS560x_print_reg16(AS5600L* as5600l,const char *formatStr, int addr, int mask) {
    int result = as560xReadReg(as5600l,addr, true, mask);
    printf(formatStr, result & mask);
}

void AS560x_print_reg8(AS5600L* as5600l,const char *formatStr, int addr, uint8_t mask) {
    uint8_t result = (uint8_t)as560xReadReg(as5600l,addr, false, mask);
    printf(formatStr, result & mask);
}


int as560xReadAngle(AS5600L* as5600l) {
    return as560xReadReg(as5600l,AS560x_RAW_ANGLE_REG, true, 0xFFF) ;
}

uint8_t as560xGetStatus(AS5600L* as5600l) {
    return (uint8_t)as560xReadReg(as5600l,AS560x_STATUS_REG, false, 0x38);
}

void sensorData(AS5600L* as5600l) {
    AS560x_print_reg8(as5600l,"Status: %02x; ", AS560x_STATUS_REG, 0x38);
    AS560x_print_reg8(as5600l,"AGC: %3x; ", 0x1a, 0xff);
    AS560x_print_reg16(as5600l,"Angle: %d\n\r", AS560x_RAW_ANGLE_REG, 0xFFF);
}