#include "debug.h"
#include "can.h"
#include "ws2812.h"
#include "memecoder.h"
#include "ctre.h"
#include "mode.h"

#include <string.h>


void init_switch_gpio() {
    // Enable GPIOB
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    // Set up switch on PB5 and PB6, use internal pull ups
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
}


u8 get_switch_state() {
    u8 state = 0;
    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)) << 0;
    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6)) << 1;
    return state;
}


int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    Delay_Init();
    USART_Printf_Init(115200);
    printf("SystemClk:%d\r\n", SystemCoreClock);

    // Inititalize hardware
    init_switch_gpio();
    memecoder_init();
    can_init();
    ctre_init();
//    ws2812_init();

    // Clear all existing ports
    ctre_port_reset_all();

    // Main loop
    u8 magic[6];

    u8 devices[8];
    u8 dev_cnt = 0;

	CanRxMsg rx_msg;

	CanTxMsg tx_msg;
	tx_msg.IDE = CAN_Id_Extended;
	tx_msg.RTR = CAN_RTR_Data;
	tx_msg.DLC = 0;

	app_state_t app;
	app.device = 0;
	app.switch_state = 0;

	mode_openloop_state_t openloop_state;
	mode_openloop_init(&app, &openloop_state);

	mode_position_state_t position_state;
	mode_position_init(&app, &position_state);

	mode_position2_state_t position2_state;
	mode_position2_init(&app, &position2_state);
    while(1)
    {
        memecoder_update();
        app.switch_state = get_switch_state();

        for(int i = 0; i < dev_cnt; i++) {
        	app.device = devices[i];
	    	printf("Device: %d, ss: %d\r\n", app.device, app.switch_state);
			mode_openloop(&app, &openloop_state);
	//        mode_position(&app, &position_state);
	//        mode_position2(&app, &position2_state);
        }

        // Read data from CAN
    	if(can_recv(&rx_msg)) {
    		u32 arbid = rx_msg.ExtId;
    		if(arbid & CTRE_TALON_SRX) {
    			// Talon waiting to be adopted, is sending magic codes
    			if((arbid & 0xFFFFFF80) == 0x0204F800) {
        			printf("ASK ADOPT!\r\n");
    				// Save magic for later
        			memcpy(magic, rx_msg.Data, 6);

    				// Request tell it we want to be on channel 6
    				tx_msg.ExtId = 0x0004F87F;
    				tx_msg.DLC = 8;
    				tx_msg.Data[0] = 0; tx_msg.Data[1] = 0; tx_msg.Data[2] = 0; tx_msg.Data[3] = 0;
    				tx_msg.Data[4] = 0; tx_msg.Data[5] = 0; tx_msg.Data[6] = 0; tx_msg.Data[7] = ctre_port;
    				CAN_Transmit(CAN1, &tx_msg);
    			}

        		// We got a response to our request to be an channel 6
        		if((arbid & 0xFFFFFF80) == 0x0204F880) {
        			printf("ADOPT!\r\n");
        			tx_msg.ExtId = 0x0004F8FF;
        			tx_msg.DLC = 8;
        			memcpy(tx_msg.Data, magic, 6);
    				tx_msg.Data[6] = 0; tx_msg.Data[7] = ctre_port;
    				CAN_Transmit(CAN1, &tx_msg);
    				ctre_port++;

    				printf("Magic: %02X %02X %02X %02X %02X %02X\r\n", magic[0], magic[1], magic[2], magic[3], magic[4], magic[5]);

//    				app.device = arbid & 0x1F;
    				devices[dev_cnt++] = arbid & 0x1F;

    				// Factory reset
    				ctre_factory_reset(app.device);

    				Delay_Ms(100);


    				// Polarity
    				ctre_set_param(app.device, 337, 1, 0, 0);

    				// Sensor
//    				ctre_set_param(app.device, 330, 0, 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 330, 1, 0, 0);
    				// P
//    				ctre_set_param(app.device, 310, 0x400000, 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 310, (uint32_t)(((float)0x400000) * 0.04), 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 311, (uint32_t)(((float)0x400000) * 0.000005), 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 312, (uint32_t)(((float)0x400000) * 0.0), 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 313, (uint32_t)(((float)0x400000) * 0.0), 0, 0);
    				Delay_Ms(10);
    				ctre_set_param(app.device, 317, 1023, 0, 0);
    				// I
//    				ctre_set_param(app.device, 311, 0x40000, 0, 0);

    				// IZone
//    				ctre_set_param(app.device, 314, 0, 0, 0);
        		}
    		}
    	}

		Delay_Ms(1);
    }
}