/*
 * canbus.c
 *
 * Created: 09/08/2018 10:13:05
 *  Author: Ledo
 */ 

#include "asf.h"
#include <can.h>
#include "globales.h"

#define CAN_COMM_TXMB_ID             0
#define CAN_TX_PRIO                  14

#define CAN_COMM_RXMB_MOTOR_ID       3
 #define CAN_COMM_RXMB_COOLER_ID     2
#define CAN_COMM_RXMB_IO_ID          1

#define CAN_NODES                    3
#define CAN_MOTOR_ID                 1539
#define CAN_IO_ID                    1537
#define CAN_COOLER_ID                1538


/** CAN frame max data length */
#define MAX_CAN_FRAME_DATA_LEN       8

/** Transfer mailbox structure */
can_mb_conf_t tx_mailbox, motor_rx_mailbox, cooler_rx_mailbox, io_rx_mailbox;


uint32_t CAN_ring_counter;

//Prototipos
void Can_init(void);
void reset_mailbox_conf(can_mb_conf_t *p_mailbox);
void decode_can_msg(can_mb_conf_t *p_mailbox);
void send_cmd2_slurry_pump(CAN_MOTOR_CMD cmd,uint8_t data);
void send_cmd2_cooler(CAN_COOLER_CMD cmd,uint16_t data);
void send_cmd2_IO(CAN_IO_CMD cmd, uint32_t mask);
uint32_t Salidas(void);
void decode_can_msg_motor(void);
void decode_can_msg_cooler(void);
void decode_can_msg_IO(void);

void CAN_Loop(void)
{
	  switch(CAN_ring_counter)
	  {
		  case 0:
		   send_cmd2_motor(Read_vars,0);
		   break;
		  case 1:
		   send_cmd2_cooler(Leer_T,0);
		  break;
		  case 2:
		  send_cmd2_IO(Outs_update_all,Salidas());
		  break;
		  
		  default:
		  break;

	  }
	  CAN_ring_counter++;
	  if(CAN_ring_counter>=CAN_NODES)
	  CAN_ring_counter = 0;
	  
	 
}

void Can_init(void)
{
	uint32_t ul_sysclk, cnt = 0;
	uint8_t uc_key;

	/* Enable CAN0 clock. */
	pmc_enable_periph_clk(ID_CAN0);

	ul_sysclk = sysclk_get_cpu_hz();
	can_init(CAN0, ul_sysclk, CAN_BPS_500K);

	/* Disable all CAN0 interrupts. */
	can_disable_interrupt(CAN0, CAN_DISABLE_ALL_INTERRUPT_MASK);

	/* Init CAN0 Mailbox 0 to Transmit Mailbox. To all nodes */
	reset_mailbox_conf(&tx_mailbox);
	tx_mailbox.ul_mb_idx = CAN_COMM_TXMB_ID;
	tx_mailbox.uc_obj_type = CAN_MB_TX_MODE;
	tx_mailbox.uc_tx_prio = CAN_TX_PRIO;
	tx_mailbox.uc_id_ver = 0;
	tx_mailbox.ul_id_msk = 0;
	can_mailbox_init(CAN0, &tx_mailbox);

	/* Init CAN0 Mailbox 3 to Reception Mailbox. Motor Card*/
	reset_mailbox_conf(&motor_rx_mailbox);
	motor_rx_mailbox.ul_mb_idx = CAN_COMM_RXMB_MOTOR_ID;
	motor_rx_mailbox.uc_obj_type = CAN_MB_RX_MODE;
	motor_rx_mailbox.uc_id_ver = 0;
	motor_rx_mailbox.ul_id_msk = CAN_MAM_MIDvA_Msk | CAN_MAM_MIDvB_Msk;
	motor_rx_mailbox.ul_id = CAN_MID_MIDvA(CAN_MOTOR_ID);
	can_mailbox_init(CAN0, &motor_rx_mailbox);

	/* Init CAN0 Mailbox 2 to Reception Mailbox. CAN_Cooler_board */
	reset_mailbox_conf(&cooler_rx_mailbox);
	cooler_rx_mailbox.ul_mb_idx = CAN_COMM_RXMB_COOLER_ID;
	cooler_rx_mailbox.uc_obj_type = CAN_MB_RX_MODE;
	cooler_rx_mailbox.uc_id_ver = 0;
	cooler_rx_mailbox.ul_id_msk = CAN_MAM_MIDvA_Msk | CAN_MAM_MIDvB_Msk;
	cooler_rx_mailbox.ul_id = CAN_MID_MIDvA(CAN_COOLER_ID);
	can_mailbox_init(CAN0, &cooler_rx_mailbox);
	
	/* Init CAN0 Mailbox 1 to Reception Mailbox. CAN_IO_Board */
	reset_mailbox_conf(&io_rx_mailbox);
	io_rx_mailbox.ul_mb_idx = CAN_COMM_RXMB_IO_ID;
	io_rx_mailbox.uc_obj_type = CAN_MB_RX_MODE;
	io_rx_mailbox.uc_id_ver = 0;
	io_rx_mailbox.ul_id_msk = CAN_MAM_MIDvA_Msk | CAN_MAM_MIDvB_Msk;
	io_rx_mailbox.ul_id = CAN_MID_MIDvA(CAN_IO_ID);
	io_rx_mailbox.uc_length = 8;
	can_mailbox_init(CAN0, &io_rx_mailbox);
			
	/* Enable CAN0 mailbox interrupts. */
	can_enable_interrupt(CAN0, 1 << CAN_COMM_RXMB_MOTOR_ID);
	can_enable_interrupt(CAN0, 1 << CAN_COMM_RXMB_COOLER_ID);
	can_enable_interrupt(CAN0, 1 << CAN_COMM_RXMB_IO_ID);

	/* Configure and enable interrupt of CAN0. */
	NVIC_EnableIRQ(CAN0_IRQn);
	
	
}

/**
 * \brief Default interrupt handler for CAN 0.
 */
void CAN0_Handler(void)
{
	uint32_t ul_status;
	
    if ( CAN0->CAN_SR & 2)
    {
	    ul_status = can_mailbox_get_status(CAN0, 1);
	    if(ul_status & CAN_MSR_MRDY)
	    {
		    io_rx_mailbox.ul_status = ul_status;
		    can_mailbox_read(CAN0, &io_rx_mailbox);
		    decode_can_msg_IO();
	    }
    }
	
	if ( CAN0->CAN_SR & 4)
	{
		ul_status = can_mailbox_get_status(CAN0, 2);
		if(ul_status & CAN_MSR_MRDY)
		{
			cooler_rx_mailbox.ul_status = ul_status;
		    can_mailbox_read(CAN0, &cooler_rx_mailbox);
		    decode_can_msg_cooler();
		}
	}
	

	
}

/**
 * \brief Decode CAN messages.
 *
 *  \param p_mailbox Pointer to CAN Mailbox structure.
 */
void decode_can_msg_motor(void)
{
	ul2byte.data32 = motor_rx_mailbox.ul_datal;
	MotorRead.Enable = ul2byte.bytes[0];
	MotorRead.Brake = ul2byte.bytes[1];
	MotorRead.Dir = ul2byte.bytes[2];
	MotorRead.Speed = ul2byte.bytes[3];
	ul2byte.data32 = motor_rx_mailbox.ul_datah;
	MotorRead.Power = ul2byte.bytes[0];
	MotorRead.T = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
	MotorRead.Hall =ul2byte.bytes[3];
	
}



void decode_can_msg_cooler(void)
{
	ul2byte.data32 = cooler_rx_mailbox.ul_datal;
	ul2byteH.data32 = cooler_rx_mailbox.ul_datah;
	switch(ul2byte.bytes[0])
	{
		case Set_Kp_A:
		IntercambiadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		IntercambiadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		IntercambiadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Ki_A:
		IntercambiadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		IntercambiadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		IntercambiadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Kd_A:
		IntercambiadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		IntercambiadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		IntercambiadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Enable_disable_A:
	    IntercambiadorRead.enable = ul2byte.bytes[1];
		break;
		case Set_T_setpoint_A:
		IntercambiadorRead.Tset = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		break;
		case Set_Kp_B:
		SecadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		SecadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Ki_B:
		SecadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		SecadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Kd_B:
		SecadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		SecadorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Enable_disable_B:
		SecadorRead.enable = ul2byte.bytes[1];
		break;
		case Set_T_setpoint_B:
		SecadorRead.Tset = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		break;
		case Set_Kp_C:
		ReactorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		ReactorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Ki_C:
		ReactorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		ReactorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Set_Kd_C:
		ReactorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		ReactorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Kd = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Enable_disable_C:
		ReactorRead.enable = ul2byte.bytes[1];
		break;
		case Set_T_setpoint_C:
		ReactorRead.Tset = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		break;
		case Enable_all:
		IntercambiadorRead.enable = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.enable = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		break;
		case Disable_all:
		IntercambiadorRead.enable = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.enable = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		break;
		case Leer_T:
		IntercambiadorRead.Tactual = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Tactual = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Tactual = ul2byteH.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case Leer_Set:
		IntercambiadorRead.Tset = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Tset = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Tset = ul2byte.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case LeerPID_A:
		IntercambiadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		IntercambiadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		IntercambiadorRead.Kd = ul2byte.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case LeerPID_B:
		SecadorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		SecadorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		SecadorRead.Kd = ul2byte.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;
		case LeerPID_C:
		ReactorRead.Kp = ul2byte.bytes[1] | (ul2byte.bytes[2] << 8);
		ReactorRead.Ki = ul2byte.bytes[3] | (ul2byteH.bytes[0] << 8);
		ReactorRead.Kd = ul2byte.bytes[1] | (ul2byteH.bytes[2] << 8);
		break;

		default:
		break;
		
	}
	
}


void decode_can_msg_IO(void)
{
	uint32_t ultemp;
	ultemp = io_rx_mailbox.ul_datal;
	ul2byte.data32 = ultemp ;

	//decodificamos el estado de las entradas digitales
	InputsState.bCAN_IN0= ultemp & CAN_INPUT0;
	InputsState.bCAN_IN1= ultemp & CAN_INPUT1;
	InputsState.bCAN_IN2= ultemp & CAN_INPUT2;
	InputsState.bCAN_IN3= ultemp & CAN_INPUT3;
	InputsState.bCAN_IN4= ultemp & CAN_INPUT4;
	InputsState.bCAN_IN5= ultemp & CAN_INPUT5;
	InputsState.bCAN_IN6= ultemp & CAN_INPUT6;
	InputsState.bCAN_IN7= ultemp & CAN_INPUT7;
	InputsState.bCAN_IN8= ultemp & CAN_INPUT8;
	InputsState.bCAN_IN9= ultemp & CAN_INPUT9;
	InputsState.bCAN_IN10= ultemp & CAN_INPUT10;
	InputsState.bCAN_IN11= ultemp & CAN_INPUT11;
	InputsState.bCAN_IN12= ultemp & CAN_INPUT12;
	InputsState.bCAN_IN13= ultemp & CAN_INPUT13;
	InputsState.bCAN_IN14= ultemp & CAN_INPUT14;
	InputsState.bCAN_IN15= ultemp & CAN_INPUT15;
	
	InputsState.PuertoA = ul2byte.bytes[0];//ultemp;// ((uint8_t *)ultemp)[0];
	InputsState.PuertoB = ul2byte.bytes[1];//ultemp>>8;//((uint8_t *)ultemp)[1];

	//decodificamos el estado de las salidas digitales
	ultemp = io_rx_mailbox.ul_datah;
	ul2byte.data32 = ultemp;
	OutsStateRead.bCAN_OUT0 = ultemp & CAN_OUTPUT0;
	OutsStateRead.bCAN_OUT1 = ultemp & CAN_OUTPUT1;
	OutsStateRead.bCAN_OUT2 = ultemp & CAN_OUTPUT2;
	OutsStateRead.bCAN_OUT3 = ultemp & CAN_OUTPUT3;
	OutsStateRead.bCAN_OUT4 = ultemp & CAN_OUTPUT4;
	OutsStateRead.bCAN_OUT5 = ultemp & CAN_OUTPUT5;
	OutsStateRead.bCAN_OUT6 = ultemp & CAN_OUTPUT6;
	OutsStateRead.bCAN_OUT7 = ultemp & CAN_OUTPUT7;
	OutsStateRead.bCAN_OUT8 = ultemp & CAN_OUTPUT8;
	OutsStateRead.bCAN_OUT9 = ultemp & CAN_OUTPUT9;
	OutsStateRead.bCAN_OUT10 = ultemp & CAN_OUTPUT10;
	OutsStateRead.bCAN_OUT11 = ultemp & CAN_OUTPUT11;
	OutsStateRead.bCAN_OUT12 = ultemp & CAN_OUTPUT12;
	OutsStateRead.bCAN_OUT13 = ultemp & CAN_OUTPUT13;
	OutsStateRead.bCAN_OUT14 = ultemp & CAN_OUTPUT14;
	OutsStateRead.bCAN_OUT15 = ultemp & CAN_OUTPUT15;
	OutsStateRead.bCAN_OUT16 = ultemp & CAN_OUTPUT16;
	OutsStateRead.bCAN_OUT17 = ultemp & CAN_OUTPUT17;
	
	OutsStateRead.PuertoC = ul2byte.bytes[0];
	OutsStateRead.PuertoD = ul2byte.bytes[1];
	OutsStateRead.PuertoR = ul2byte.bytes[2];
}



/**
 * \brief Reset mailbox configure structure.
 *
 *  \param p_mailbox Pointer to mailbox configure structure.
 */
void reset_mailbox_conf(can_mb_conf_t *p_mailbox)
{
	p_mailbox->ul_mb_idx = 0;
	p_mailbox->uc_obj_type = 0;
	p_mailbox->uc_id_ver = 0;
	p_mailbox->uc_length = 0;
	p_mailbox->uc_tx_prio = 0;
	p_mailbox->ul_status = 0;
	p_mailbox->ul_id_msk = 0;
	p_mailbox->ul_id = 0;
	p_mailbox->ul_fid = 0;
	p_mailbox->ul_datal = 0;
	p_mailbox->ul_datah = 0;
}


void send_cmd2_slurry_pump(CAN_MOTOR_CMD cmd,uint8_t data)
{
    tx_mailbox.ul_id = CAN_MID_MIDvA(CAN_MOTOR_ID);
    tx_mailbox.uc_length = MAX_CAN_FRAME_DATA_LEN;
    tx_mailbox.ul_datal = (uint32_t) ((data<<8) | cmd);
    tx_mailbox.ul_datah = 0x00u;
	can_mailbox_write(CAN0, &tx_mailbox);
	/* Send out the information in the mailbox. */
	can_global_send_transfer_cmd(CAN0, CAN_TCR_MB0);
}


void send_cmd2_cooler(CAN_COOLER_CMD cmd,uint16_t data)
{

	tx_mailbox.ul_id = CAN_MID_MIDvA(CAN_COOLER_ID);
	tx_mailbox.uc_length = MAX_CAN_FRAME_DATA_LEN;
	tx_mailbox.ul_datal = (uint32_t) ((data<<8) | cmd);
	tx_mailbox.ul_datah = 0x00u;
	can_mailbox_write(CAN0, &tx_mailbox);
	/* Send out the information in the mailbox. */
	can_global_send_transfer_cmd(CAN0, CAN_TCR_MB0);
}


void send_cmd2_IO(CAN_IO_CMD cmd, uint32_t mask)
{

	tx_mailbox.ul_id = CAN_MID_MIDvA(CAN_IO_ID);
	tx_mailbox.uc_length = MAX_CAN_FRAME_DATA_LEN;
	tx_mailbox.ul_datal = ((mask << 8) | cmd);
	tx_mailbox.ul_datah = 0x00u;
	can_mailbox_write(CAN0, &tx_mailbox);
	/* Send out the information in the mailbox. */
	can_global_send_transfer_cmd(CAN0, CAN_TCR_MB0);
}


//Retorna la mascara de control de las salidas de acuerdo al estado de la
// structura OutsState.
uint32_t Salidas(void)
{
	uint32_t ultemp;
	ultemp = ((OutsStateRead.PuertoR & 0b11) << 16) | (OutsStateRead.PuertoD << 8) | OutsStateRead.PuertoC;
	return ultemp;		 
}


 

