#include "PowerControl.h"
#include "stdint.h"
#include "math.h"


void power_calu(power_model_t* param,float* in_array,float* rpm_array)
{
  float InitialGivePower[param->motor_num];
  float totalpower=0;
  float in[4];
  float ve[4];
	for (uint8_t i = 0; i < param->motor_num; i++) // first get all the initial motor power and total motor power
	{
    in[i] = in_array[i];
    ve[i] = rpm_array[i];
		InitialGivePower[i] = in[i] * param->toque_coefficient * ve[i] +
    param->k2 * ve[i] * ve[i]+ param->k1 * in[i]*in[i] + param->constant;

    param->power[i] = InitialGivePower[i];
		if (InitialGivePower[i] < 0) // negative power not included (transitory)
			continue;
		totalpower += InitialGivePower[i];
	}
  param->total_power = totalpower;
}
float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power)
{
  float total_power=0;
  for(uint8_t i=0;i<num;i++)
  {
    total_power+=param_array[i]->total_power;
  }
  if(total_power>max_power)
  {return max_power/total_power;}
  else
  {return 2;}
}

void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array)
{
  float in[4];
  float ve[4];
  if(scale<1)
  {

    float ScaledGivePower[4];
	  for (uint8_t i = 0; i < param->motor_num; i++)
	  {
    in[i] = in_array[i];
    ve[i] = rpm_array[i];
			ScaledGivePower[i] = param->power[i] * scale; // get scaled power
			if (ScaledGivePower[i] < 0)
			{
				continue;
			}
			float b = param->toque_coefficient * ve[i];
			float c = param->k2 * ve[i]*ve[i] - ScaledGivePower[i] + param->constant;
			float inside = b * b - 4 * param->k1 * c;
			if (inside < 0)
			{
				continue;
			}
			else if (in[i] > 0) // Selection of the calculation formula according to the direction of the original moment
			{
				float temp = (-b + sqrt(inside)) / (2 * param->k1);
				if (temp > 16000)
				{
					out_array[i] = 16000;
				}
				else
					out_array[i] = temp;
			}
			else
			{
				float temp = (-b - sqrt(inside)) / (2 * param->k1);
				if (temp < -16000)
				{
					out_array[i] = -16000;
				}
				else
					out_array[i] = temp;
			}
		}
  }
}