#include "mot.h"

#include "pwm.h"
  const unsigned long count = 4;
 short pwm[count] {0,0,0,0};

void QUAD_Set(short throt, short x, short y, short z)
{
  //  3   2
  //  4   1
  const unsigned char ESC_UL = 2;  // CW  // M3
  const unsigned char ESC_UR = 1;  // CCW // M2
  const unsigned char ESC_DL = 3;  // CCW // M4
  const unsigned char ESC_DR = 0;  // CW  // M1
  
  

  
    for(int a=0; a<count; a++) pwm[a] = 0;
  
  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
                        
  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
  
  pwm[ESC_UL] += z; pwm[ESC_UR] -= z;
  pwm[ESC_DL] -= z; pwm[ESC_DR] += z;
  
  short min=500, max=-500;
  for(int a=0; a<count; a++)
  {
    if(pwm[a]>500) pwm[a]=500;
    else if(pwm[a]<-500) pwm[a]=-500;
    //---
    if(min>pwm[a]) min=pwm[a];
    if(max<pwm[a]) max=pwm[a];
  }
  
  short up=(throt+max)-1000;
  short down=(throt+min);
  if(up>0) throt-=up;
  if(down<0) throt-=down;
  
  for(int a=0; a<count; a++) pwm[a] += throt;
  
  PWM_SetQuad(pwm, 0, 1000);
}