#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>


#include "AP_UAVCAN.h"
#include "HVcallback.h"
#define REG_MASK 0xFFF8U
#define TYPE_MASK 0x38U




//hight volt HighVoltThrusterSetRegs.hpp
void HVregs_v5y4(uint8_t _buffer[],int32_t data[]){

	uint16_t revID = _buffer[0]+(_buffer[1]<<8);
	uint16_t regID = revID & REG_MASK;
	uint8_t typeID;

    typeID = (uint8_t)revID & TYPE_MASK;
	
	switch (typeID)
	{

		case TYPE_DATA_8BIT:
		 {
		   
			 switch (regID)
			 {
			   
	
			   case MC_REG_CONTROL_MODE:	
			   case MC_REG_RUC_STAGE_NBR:
			   	{
				   data[0] = (int32_t)_buffer[2];
				   break;
			    }
	
			   default:
			   {
				 data[0] = 0;
				 break;
			   }
			
		     }
		   
		   break;
		 }
	
		 case TYPE_DATA_16BIT:
		 { 
		  
			 switch (regID)
			 {
			   case MC_REG_SPEED_KP:
			   case MC_REG_SPEED_KI:
			   case MC_REG_SPEED_KD:
		   	   case MC_REG_I_Q_KP:
		       case MC_REG_I_Q_KI:
		       case MC_REG_I_Q_KD:
		       case MC_REG_I_D_KP:
		       case MC_REG_I_D_KI:
		       case MC_REG_I_D_KD:
			   case MC_REG_I_A:
			   case MC_REG_I_B:	
			   case MC_REG_I_ALPHA_MEAS:
			   case MC_REG_I_BETA_MEAS:	
			   case MC_REG_I_Q_MEAS:	
			   case MC_REG_I_D_MEAS:
			   case MC_REG_I_Q_REF:	
			   case MC_REG_I_D_REF:	
			   case MC_REG_V_Q:	
			   case MC_REG_V_D:	
			   case MC_REG_V_ALPHA:	
			   case MC_REG_V_BETA:
			   case MC_REG_STOPLL_EL_ANGLE:	
			   case MC_REG_STOPLL_ROT_SPEED:	
			   case MC_REG_STOPLL_I_ALPHA:	
			   case MC_REG_STOPLL_I_BETA:	
			   case MC_REG_STOPLL_BEMF_ALPHA:	
			   case MC_REG_STOPLL_BEMF_BETA:	
			   case MC_REG_STOPLL_C1:
			   case MC_REG_STOPLL_C2:
			   case MC_REG_STOPLL_KI:
			   case MC_REG_STOPLL_KP:
			   case MC_REG_SPEED_KP_DIV:
			   case MC_REG_SPEED_KI_DIV:	
			   case MC_REG_SPEED_KD_DIV:
			   case MC_REG_I_D_KP_DIV:	
			   case MC_REG_I_D_KI_DIV:	
			   case MC_REG_I_D_KD_DIV:	
			   case MC_REG_I_Q_KP_DIV:	
			   case MC_REG_I_Q_KI_DIV:	
			   case MC_REG_I_Q_KD_DIV:	
			   case MC_REG_STOPLL_KI_DIV:	
			   case MC_REG_STOPLL_KP_DIV:
			   	{
					data[0] = (int32_t)(_buffer[2] + (_buffer[3]<<8));
			   		break;
			   	}
	
			   default:
			   {
				 data[0] = 0;
				 break;
			   }
		     }
			 
		   break;
		 }
	
		 case TYPE_DATA_32BIT:
		 {

			 switch (regID)
			 {
			   	
			   case MC_REG_SPEED_REF:
			   case MC_REG_STOPLL_EST_BEMF:			   	
			   case MC_REG_STOPLL_OBS_BEMF:
			   {
				 	data[0] = (int32_t)(_buffer[2] + (_buffer[3]<<8) + (_buffer[4]<<16) +(_buffer[5]<<24));   
				 break;
			   }
	
			   default:
			   {
				 data[0] = 0;
				 break;
			    }
			  }
			 break;
		   }
		   
		   
		 
	
		  case TYPE_DATA_RAW:
		 {
		   /* First 2 bytes of the answer is reserved to the size */
	  
	
		   switch (regID)
		   {
			 
			 case MC_REG_SPEED_RAMP:
			 {
			   if(_buffer[2]==0x5A)
			   	{
				   data[0] =(int32_t)( _buffer[3] + _buffer[4]*256 +(_buffer[5]<<16) +(_buffer[6]<<24));
			 	}else if(_buffer[2]==0xA5){
					data[1] = (int32_t)(_buffer[3] +( _buffer[4]<<8));
						
			 	}
			
				 
			   break;
			  }
	
			 case MC_REG_TORQUE_RAMP:
			 {
			   
				 data[0] = (int32_t)(_buffer[2] +( _buffer[3]<<8));
				 data[1] = (int32_t)(_buffer[4] +( _buffer[5]<<8));

			   break;
			 }
	
			 case MC_REG_REVUP_DATA:
			 {

				   if( _buffer[2] !=0xFF){
				   	 data[0] =(int32_t)( _buffer[3] + _buffer[4]*256 +(_buffer[5]<<16) +(_buffer[6]<<24));
				   	}
				   else if( _buffer[2] ==0xFF){
				   	 data[1] = (int32_t)(_buffer[3] +( _buffer[4]<<8));
					 data[2] = (int32_t)(_buffer[5] +( _buffer[6]<<8));
				   	}
			   
			   break;
			 }
	
			 case MC_REG_CURRENT_REF:
			 {
			   data[0] = (int32_t)(_buffer[2] +( _buffer[3]<<8));
			   data[1] = (int32_t)(_buffer[4] +( _buffer[5]<<8));
			   break;
	           }
	
			
			 default:
			 {
			   data[0] = 0;
			   break;
			 }
		   }
	
		  
		   break;
		 }
	
		 default:
		 {
		   data[0] = 0;
		   break;
		 }
	   }

}



void HVcycle(uint8_t _code,uint8_t _buffer[6],HVmes &Thruster){
	switch (_code)
		{
			case 0x03:
				{
					Thruster.voltage = _buffer[0]+(_buffer[1]<<8);
					Thruster.speed = _buffer[2]+(_buffer[3]<<8) + (_buffer[4]<<16) + (_buffer[5]<<24);
					
				}
				break;
			case 0x05:
				{
					Thruster.fault = _buffer[0]+(_buffer[1]<<8) + (_buffer[2]<<16)+(_buffer[3]<<24);
					Thruster.state =  _buffer[4] + (_buffer[5]<<8);
					
				}
				break;
			case 0x0A:
				{
					Thruster.power = _buffer[0]+(_buffer[1]<<8) ;
					Thruster.temperature =  _buffer[2] + (_buffer[3]<<8);
					
					
					
					
					
				}
				break;
			default:
				break;
		}

}