/*
 * accel.c
 *
 *  Created on: Oct 13, 2024
 *      Author: true
 */

#include "ch32x035_conf.h"

#include "accel.h"
#include "lis2hh12_reg.h"

#include "comms/spi_master.h"

#include "misc/i8atan2.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>



// user data
AccelData accel;

uint8_t accel_found = 0;

uint8_t movement_idx;       // index into "read" register
int16_t movement_read[4];   // last read movement value
int16_t movement;           // last calculated movement value
uint16_t movement_worst;    // worst seen movement value


// hardware
static stmdev_ctx_t dev_ctx;



static int32_t accel_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
    (void)(handle);

    // i2c_write_reg8(LIS2_I2C_ADDR_SDO_LOW, reg, bufp, len);
    LIS2_CS_PORT->BCR = LIS2_CS_PIN;
    spim_write_reg8(reg, bufp, len);
    LIS2_CS_PORT->BSHR = LIS2_CS_PIN;

    return 0;
}

static int32_t accel_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
    (void)(handle);

    // i2c_read_reg8(LIS2DW_I2C_ADDR_SDO_LOW, reg, bufp, len);
    LIS2_CS_PORT->BCR = LIS2_CS_PIN;
    spim_read_reg8(reg, bufp, len);
    LIS2_CS_PORT->BSHR = LIS2_CS_PIN;

    return 0;
}


void accel_init()
{
    uint8_t devid;
    uint8_t reset;
    uint16_t timeout = 1000;

    dev_ctx.write_reg = accel_write;
    dev_ctx.read_reg = accel_read;

    // make sure we've got the right device
    lis2hh12_dev_id_get(&dev_ctx, &devid);
    if (devid == LIS2HH12_ID) {
        accel_found = 1;
    }

    // reset accelerometer
    if (accel_found) {
        lis2hh12_dev_reset_set(&dev_ctx, PROPERTY_ENABLE);
        do {
            lis2hh12_dev_reset_get(&dev_ctx, &reset);
            if (!reset) {
                timeout = 0;
                accel_found = 1;
            }
        } while (timeout--);
    }

    if (accel_found) {
        // disable block update
        // data in output registers is updated immediately; FIFO is disabled
        lis2hh12_block_data_update_set(&dev_ctx, PROPERTY_DISABLE);

        // configure scale, power mode
        lis2hh12_xl_full_scale_set(&dev_ctx, LIS2HH12_2g);
        // TODO: check this
        // lis2hh12_power_mode_set(&dev_ctx, LIS2DW12_CONT_LOW_PWR_LOW_NOISE_4);

        // configure filter chain
        // low pass filter enabled for 6D (not currently used)
        lis2hh12_xl_filter_out_path_set(&dev_ctx, LIS2HH12_FILT_LP);
        // digital LPF2 filter of output data
        lis2hh12_xl_filter_low_bandwidth_set(&dev_ctx, LIS2HH12_LP_ODR_DIV_9);

        // configure output data rate
        lis2hh12_xl_data_rate_set(&dev_ctx, LIS2HH12_XL_ODR_200Hz);
    }
}

void accel_poll()
{
    uint8_t reg = 1;
    int16_t xyz[3];

    if (!accel_found) return;

    while (reg) {
        // read output only if new value is available
        lis2hh12_xl_flag_data_ready_get(&dev_ctx, &reg);

        if (reg) {
            // read acceleration data
            memset(xyz, 0x00, 3 * sizeof(int16_t));
            lis2hh12_acceleration_raw_get(&dev_ctx, xyz);

            accel.x = xyz[0] >>= 8;
            accel.y = xyz[1] >>= 8;
            accel.z = xyz[2] >>= 8;

            // compute our shitty "movement" thing
            // awful way to detect being still
            reg = sizeof(movement_read) / sizeof(movement_read[0]);

            movement_idx++;
            if (movement_idx == reg) {
                movement_idx = 0;
                movement = abs(movement_read[3] - movement_read[0]);
                if (movement > movement_worst) {
                    movement_worst = movement;
                }
            }

            movement_read[movement_idx] = abs(accel.x) + abs(accel.y);
        }
    }
}

int8_t accel_get_rotation(struct AccelData *a)
{
    int8_t nx, ny, ret;

    nx = -a->x;
    ny =  a->y;

    ret = i8atan2(nx, ny) >> 1;
    if (ret < 0) {
        ret += 128;
    }

    return ret;
}

int16_t accel_get_movement()
{
    return movement;
}