/** LAB4 mems SPI sensor template
 *  Hardware platform: STM32F411E-DISCO
 *  PA2 Port - USART TXD should be connected to serial<->usb converter
 */
#include <config/conf.h>
#include <periph/gpio/gpio.hpp>
#include <foundation/sys/dbglog.h>
#include <periph/drivers/serial/uart_early.hpp>
#include <periph/drivers/spi/spi_master.hpp>
#include <isix.h>
#include <periph/dma/dma.hpp>
#include <cstring>
#include <config/conf.h>
#include <periph/gpio/gpio.hpp>
#include <foundation/sys/dbglog.h>
#include <periph/drivers/i2c/i2c_master.hpp>
#include <isix.h>
#include <cstring>
#include <config/conf.h>
#include <foundation/sys/dbglog.h>
#include <stm32_ll_usart.h>
#include <stm32_ll_tim.h>
#include <stm32_ll_system.h>
#include <stm32_ll_bus.h>
#include <isix/arch/irq.h>
#include <stdio.h>
#include <string.h>


#define ADDR_WHO_AM_I 0x0F
#define DIR_READ 0x80
#define IADDR_INCREMENT 0x40
#define L3G20_CTRL_REG1 0x20
#define L3G20_CTRL_REG2 0x21
#define L3G20_CTRL_REG3 0x22
#define L3G20_CTRL_REG4 0x23
#define L3G20_CTRL_REG5 0x24
#define L3G20_OUT_X_L 0x28
#define L3G20_OUT_X_H 0x29
#define L3G20_OUT_Y_L 0x2A
#define L3G20_OUT_Y_H 0x2B
#define L3G20_OUT_Z_L 0x2C
#define L3G20_OUT_Z_H 0x2D


namespace {
    /* Initialize the debug USART */
    auto usart_protected_init() -> void {
        static isix::mutex m_mtx;
        dblog_init_locked(
                [](int ch, void*) {
                    return periph::drivers::uart_early::putc(ch);
                },
                nullptr,
                []() { m_mtx.lock();  },
                []() { m_mtx.unlock(); },
                periph::drivers::uart_early::open, "serial0", 115200
        );
    }




    //Task for the led blinking
    auto watch_sensor() -> void {
        namespace opt = periph::option;
        int ret {};
        periph::drivers::spi_master spi("spi1");
        do {
            if((ret=spi.set_option(opt::speed(10E6)))<0) {
                break;
            }
            if((ret=spi.set_option(opt::polarity(opt::polarity::low)))<0) {
                break;
            }
            if((ret=spi.set_option(opt::phase(opt::phase::_1_edge)))<0) {
                break;
            }
            if((ret=spi.set_option(opt::dwidth(8)))<0) {
                break;
            }
            if((ret=spi.set_option(opt::bitorder(opt::bitorder::msb)))<0) {
                break;
            }
            if((ret=spi.open(ISIX_TIME_INFINITE))<0) {
                break;
            }
            //TODO: LAB4 user code here


LL_GPIO_SetPinMode(GPIOD,LL_GPIO_PIN_12,LL_GPIO_MODE_OUTPUT);
LL_GPIO_SetPinMode(GPIOD,LL_GPIO_PIN_13,LL_GPIO_MODE_OUTPUT);
LL_GPIO_SetPinMode(GPIOD,LL_GPIO_PIN_14,LL_GPIO_MODE_OUTPUT);
LL_GPIO_SetPinMode(GPIOD,LL_GPIO_PIN_15,LL_GPIO_MODE_OUTPUT);



            const uint8_t inbuf1[2]{ADDR_WHO_AM_I | DIR_READ};
            uint8_t outbuf1[2]{};
            static_assert(sizeof outbuf1 == sizeof inbuf1);
            static constexpr auto csno = 0;

            periph::blk::trx_transfer tran1(inbuf1,outbuf1,sizeof outbuf1);
            int ret = spi.transaction(csno,tran1);

 dbg_info("outbuf[1] = %x\n",outbuf1[1]);
            if (outbuf1[1] != 0xD4)
            {
                dbg_info("Urzadzenie nie podclaczone \n");
                break;
            }

        uint8_t inbuf2[2]{L3G20_CTRL_REG1 | DIR_READ};
            uint8_t outbuf[2]{};
            periph::blk::trx_transfer tran2(inbuf2,outbuf,sizeof outbuf1);
            ret= spi.transaction(csno,tran2);
            dbg_info("Odczytano reg1 = %x %x\n",outbuf[0],outbuf[1]);

            uint8_t inbuf3[2]{L3G20_CTRL_REG1 | 0x0F};
            periph::blk::trx_transfer tran3(inbuf3,outbuf,sizeof outbuf);
            ret= spi.transaction(csno,tran3);
            dbg_info("Odczytano reg1 = %x %x\n",outbuf[0],outbuf[1]);

            uint8_t inbuf4[2]{L3G20_CTRL_REG2 | 0x00};
            periph::blk::trx_transfer tran4(inbuf4,outbuf,sizeof outbuf);
            ret= spi.transaction(csno,tran4);

            uint8_t inbuf5[2]{L3G20_CTRL_REG3 | 0b00001000};
            periph::blk::trx_transfer tran5(inbuf5,outbuf,sizeof outbuf);
            ret= spi.transaction(csno,tran5);

            uint8_t inbuf6[2]{L3G20_CTRL_REG4 | 0b00001000};
            periph::blk::trx_transfer tran6(inbuf6,outbuf,sizeof outbuf);
            ret= spi.transaction(csno,tran6);

            uint8_t inbuf7[2]{L3G20_CTRL_REG5 | 0x00};
            periph::blk::trx_transfer tran7(inbuf7,outbuf,sizeof outbuf);
            ret= spi.transaction(csno,tran7);

 while(1)
            {
                constexpr uint8_t inbuf9[7]={L3G20_OUT_X_L | DIR_READ | IADDR_INCREMENT};
                uint8_t outbuf9[7] {};
               // enum {na,xla,xha,yla,yha,zla,zha};
                static_assert(sizeof outbuf9 == sizeof inbuf9);
                periph::blk::trx_transfer tran9(inbuf9,outbuf9,sizeof outbuf9);
                ret=spi.transaction(csno,tran9);

                int16_t val_x=(int16_t(outbuf9[2]<<8|int16_t(outbuf9[1])));
                int16_t val_y=(int16_t(outbuf9[4]<<8|int16_t(outbuf9[3])));
                int16_t val_z=(int16_t(outbuf9[6]<<8|int16_t(outbuf9[5])));
    dbg_info("L3G20 position : x = %i, y = %i, z = %i", val_x,val_y,val_z);

if (abs(val_x)>300) LL_GPIO_TogglePin(GPIOD,LL_GPIO_PIN_12); else LL_GPIO_ResetOutputPin(GPIOD,LL_GPIO_PIN_12);
if (abs(val_y)>300) LL_GPIO_TogglePin(GPIOD,LL_GPIO_PIN_13); else LL_GPIO_ResetOutputPin(GPIOD,LL_GPIO_PIN_12);
if (abs(val_z)>300) LL_GPIO_TogglePin(GPIOD,LL_GPIO_PIN_14); else LL_GPIO_ResetOutputPin(GPIOD,LL_GPIO_PIN_12);

isix::wait_ms(250);
            }





        } while(0);
        dbg_info("Task exit with code %i", ret);
    }

    }


// Start main function
auto main() -> int
{
    usart_protected_init();
    // Wait some time before startup
    isix::wait_ms(500);
    static constexpr auto tsk_stack = 1024;
    static constexpr auto tsk_prio = 6;
    static auto watch_task =
        isix::thread_create_and_run(tsk_stack,tsk_prio,0,watch_sensor);
    dbprintf("<<<< LAB4 SPI MEMS sensor demo >>>>");
    isix::start_scheduler();
}