#include <string.h>
#include <stdlib.h>

#include "gpio.h"

#include "spi.h"
#include "tick.h"

#include "laser.h"

static void WriteReg(char reg, char value)
{
  char send[2]={reg | 0x80, value};
  SPI2_TransferCons(send, 2, 0, 0);
}

bool FLOW_Init() // PMW3901
{
  SPI2_Init();
  
  WriteReg(0x3A, 0x5A);
  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }

  // Test the SPI communication, checking chipId and inverse chipId
  
  char reg[4]={0x00, 0, 0x5F, 0};
   char result[4]={0, 0,0,0};
  SPI2_TransferParallel(reg, result, 4);
  
  if (result[1] != 0x49 || result[3] != 0xB6) return false;
  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
  
  WriteReg(0x7F, 0x00);
  WriteReg(0x61, 0xAD);
  WriteReg(0x7F, 0x03);
  WriteReg(0x40, 0x00);
  WriteReg(0x7F, 0x05);
  WriteReg(0x41, 0xB3);
  WriteReg(0x43, 0xF1);
  WriteReg(0x45, 0x14);
  WriteReg(0x5B, 0x32);
  WriteReg(0x5F, 0x34);
  WriteReg(0x7B, 0x08);
  WriteReg(0x7F, 0x06);
  WriteReg(0x44, 0x1B);
  WriteReg(0x40, 0xBF);
  WriteReg(0x4E, 0x3F);
  
  WriteReg(0x7F, 0x08);
  WriteReg(0x65, 0x20);
  WriteReg(0x6A, 0x18);
  WriteReg(0x7F, 0x09);
  WriteReg(0x4F, 0xAF);
  WriteReg(0x5F, 0x40);
  WriteReg(0x48, 0x80);
  WriteReg(0x49, 0x80);
  WriteReg(0x57, 0x77);
  WriteReg(0x60, 0x78);
  WriteReg(0x61, 0x78);
  WriteReg(0x62, 0x08);
  WriteReg(0x63, 0x50);
  WriteReg(0x7F, 0x0A);
  WriteReg(0x45, 0x60);
  WriteReg(0x7F, 0x00);
  WriteReg(0x4D, 0x11);
  WriteReg(0x55, 0x80);
  WriteReg(0x74, 0x1F);
  WriteReg(0x75, 0x1F);
  WriteReg(0x4A, 0x78);
  WriteReg(0x4B, 0x78);
  WriteReg(0x44, 0x08);
  WriteReg(0x45, 0x50);
  WriteReg(0x64, 0xFF);
  WriteReg(0x65, 0x1F);
  WriteReg(0x7F, 0x14);
  WriteReg(0x65, 0x60);
  WriteReg(0x66, 0x08);
  WriteReg(0x63, 0x78);
  WriteReg(0x7F, 0x15);
  WriteReg(0x48, 0x58);
  WriteReg(0x7F, 0x07);
  WriteReg(0x41, 0x0D);
  WriteReg(0x43, 0x14);
  WriteReg(0x4B, 0x0E);
  WriteReg(0x45, 0x0F);
  WriteReg(0x44, 0x42);
  WriteReg(0x4C, 0x80);
  WriteReg(0x7F, 0x10);
  WriteReg(0x5B, 0x02);
  WriteReg(0x7F, 0x07);
  WriteReg(0x40, 0x41);
  WriteReg(0x70, 0x00);
  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
  
  WriteReg(0x32, 0x44);
  WriteReg(0x7F, 0x07);
  WriteReg(0x40, 0x40);
  WriteReg(0x7F, 0x06);
  WriteReg(0x62, 0xf0);
  WriteReg(0x63, 0x00);
  WriteReg(0x7F, 0x0D);
  WriteReg(0x48, 0xC0);
  WriteReg(0x6F, 0xd5);
  WriteReg(0x7F, 0x00);
  WriteReg(0x5B, 0xa0);
  WriteReg(0x4E, 0xA8);
  WriteReg(0x5A, 0x50);
  WriteReg(0x40, 0x80);
  
  return true;
}

bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual)
{
  char reg[12]={0x02, 0, 0x03, 0, 0x04, 0, 0x05, 0, 0x06, 0, 0x07, 0};
  
  SPI2_TransferParallel(reg, reg, 12);
  
  short x = ((short)reg[5] << 8) | reg[3];
  short y = ((short)reg[9] << 8) | reg[7];
  
  if(dX) *dX = x;
  if(dY) *dY = y;
  
  if(qual) *qual = reg[11];
  
  return reg[1] & 0x80;
}